Jetcalc

Jetcalc is designed to take a model with more than six jets, and use it to give a certain acceleration and torque. It will minimize the sum of the squares of the jet powers.

I'd explain how to use it, but you're probably better off reverse engineering Hand.

I think I'm too new here to be allowed to upload or something, so make a folder labeled DanielLC in your Data folder, and put these files in it:

jetcalc.lua

Code:

require("DanielLC/matrix.lua")
require("DanielLC/quaternion.lua")

function center()
  local s={0,0,0}
  for i=0, _CHIPS() do
    s[1]=s[1]+_M(i)*_X(i)
    s[2]=s[2]+_M(i)*_Y(i)
    s[3]=s[3]+_M(i)*_Z(i)
  end
  s[1]=s[1]/_WEIGHT()
  s[2]=s[2]/_WEIGHT()
  s[3]=s[3]/_WEIGHT()
  return(s)
end
function leastsquares(matrix, rows, cols, vector)         --vector=matrix*v, returns v that minimizes vector.
  local m=trans(matrix,rows,cols)
  m=mmult(m,cols,rows,matrix,rows,cols)
  m=minv(m,cols)
  m=mmult(m,cols,cols,trans(matrix,rows,cols),cols,rows)
  local v=mvmult(m,cols,rows,vector)
  return v
end
function radius(jet,cog)
  return({_X(jet)-cog[1],_Y(jet)-cog[2],_Z(jet)-cog[3]})
end
function pmi(chip)      -- principal moment of inertia
   local typ = _TYPE(chip)
   if typ == 0 or typ == 1 or typ == 2 or typ == 6 then
      return {0.7770,1.5120,0.7770}
   elseif typ == 33 or typ == 34 or typ == 35 then
      return {0.3806,0.7560,0.3806}
   elseif typ == 3 then
      return {1.5109,0.4771,1.5109}
   elseif typ == 4 or typ == 5 then
      local op = _OPTION(chip)
      if op == 0 then
         return {0.8577,1.6450,0.8577}
      elseif op == 1 then
         return {1.8669,3.7216,1.8669}
      elseif op == 2 then
         return {3.3100,6.6162,3.3100}
      end
   elseif typ == 8 then
      local op = _OPTION(chip)
      if op == 0 then op = 1 end
      return {3.1080*op,6.0480*op,3.1080*op}
   elseif typ == 7 then
      return {0.8577,1.6540,0.8577}
   elseif typ == 10 then
      return {1.5540,3.0240,1.5540}
   end
end
function mit(chip,cog)      --Moment of inerta tensor
  local D = diag(pmi(chip),3)
  local Rot = mrot(_EX(chip),_EY(chip),_EZ(chip))
  local T = mmult(Rot,3,3,D,3,3)
  D = diag({(_EX(chip)-cog[1])^2,(_EY(chip)-cog[2])^2,(_EZ(chip)-cog[3])^2},3)
  return madd(T,D,3,3)
end
function invmit(cog)      --This is for the whole model, rather than one chip.
  local M = makeM(3,3)
  for i=0, _CHIPS() do
    madd(M,mit(i,cog),3,3)
  end
  return minv(M,3)
end
function center()
  local s={0,0,0}
  for i=0, _CHIPS() do
    s[1]=s[1]+_M(i)*_X(i)
    s[2]=s[2]+_M(i)*_Y(i)
    s[3]=s[3]+_M(i)*_Z(i)
  end
  s[1]=s[1]/_WEIGHT()
  s[2]=s[2]/_WEIGHT()
  s[3]=s[3]/_WEIGHT()
  return(s)
end
function angacc(alpha,cog)   --Converts angular velocity to angular acceleration
  return mvmult(invmit(cog),3,3,alpha)
end
function radius(jet,cog)
  return({_X(jet)-cog[1],_Y(jet)-cog[2],_Z(jet)-cog[3]})
end
function jetcalc(Ji,thrust,jets)   --For when you have at least six jets
  local HIGHT=6
  local WIDTH=jets+1
  local cog=center()
  local M=makeM(HIGHT,WIDTH)
  local i
  local j
  for j=1,jets do
    dir=rot({0,1,0,0},{_QX(Ji[j]),_QY(Ji[j]),_QZ(Ji[j]),_QW(Ji[j])})   --dir is the way jet j is facing.
    M[1][j]=dir[1]                     --the x, y, and z components of the engines must add to (0,y,0)
    M[2][j]=dir[2]
    M[3][j]=dir[3]
    torque=cross(dir,radius(Ji[j],cog))
    M[4][j]=torque[1]
    M[5][j]=torque[2]
    M[6][j]=torque[3]
  end
  for i=1,HIGHT do
    M[i][WIDTH]=thrust[i]*_WEIGHT()*6
  end
  local t=angacc({thrust[4],thrust[5],thrust[6]},cog)
  for i=1,3 do
    M[i+3][WIDTH]=t[i]
  end
  rref(M,HIGHT,WIDTH)
  local Sg,sp,rows,cols=solve(M,HIGHT,WIDTH)            --Sg = general solution (matrix), sp = particular solution (vector)
                           --for all v, J = Sg * v + sp
  local v2=leastsquares(Sg,rows,cols,vminus(sp,rows))         --returns v2 such that |Sg * v2 - (-sp)| is minimized
  local del=mvmult(Sg,rows,cols,v2)
  local Jo=vadd(del,sp,rows)
  return Jo
end
function jetcalc2(Ji,thrust)      --For when you have exactly six jets
  local cog=center()
  local HIGHT=6
  local WIDTH=7
  local M=makeM(HIGHT,WIDTH)
  local i
  local j
  for j=1,HIGHT do
    dir=rot({0,1,0,0},{_QX(Ji[j]),_QY(Ji[j]),_QZ(Ji[j]),_QW(Ji[j])})
    M[1][j]=dir[1]
    M[2][j]=dir[2]
    M[3][j]=dir[3]
    torque=cross(dir,radius(Ji[j],cog))
    M[4][j]=torque[1]
    M[5][j]=torque[2]
    M[6][j]=torque[3]
  end
  for i=1,3 do
    M[i][WIDTH]=thrust[i]*_WEIGHT()*6
  end
  local t=angacc({thrust[4],thrust[5],thrust[6]},cog)
  for i=1,3 do
    M[i+3][WIDTH]=t[i]
  end
  local Jo=makeV(HIGHT)
  rref(M,HIGHT,WIDTH)
  for i=1,HIGHT do
    Jo[i]=M[i][WIDTH]
  end
  return Jo
end

matrix.lua

Code:

M=function(i,m,MATRIX,WIDTH)            --Multiply row i by m
  local j
  for j=1,WIDTH do
    MATRIX[i][j]=m*MATRIX[i][j]
  end
end
A=function(i,j,m,MATRIX,WIDTH)            --Add m*row i to row j  YOU ARE HERE
  local c
  for c=1,WIDTH do
    MATRIX[j][c]=m*MATRIX[i][c]+MATRIX[j][c]
  end
end
function rref(MATRIX,HIGHT,WIDTH)         --Reduce
  local lead=1
  local i
  local j
  for j=1,WIDTH-1 do
    if MATRIX[lead][j]~=0 then
      M(lead,1/MATRIX[lead][j],MATRIX,WIDTH)
      for i=1,HIGHT do
        if i~=lead then
          A(lead,i,-MATRIX[i][j],MATRIX,WIDTH)
        end
      end
      if lead>=HIGHT then
        break
      end
      lead=lead+1
    end
  end
end
function makeV(size)
  local i
  local vector={}
  for i=1,size do
    table.insert(vector,0)
  end
  return vector
end
function makeM(rows,cols)
  local matrix={}
  local i
  local j
  for i=1,rows do
    table.insert(matrix,{})
    for j=1,cols do
      table.insert(matrix[i],0)
    end
  end
  return matrix
end
function solve(m0,rows,cols)               --takes an aumented matrix in RREF, and returns the general and particular solutions
  local m1=makeM(cols-1,cols-rows-1)
  local v=makeV(cols-1)
  local i
  local j
  for i=1,rows do
    for j=1,cols-rows-1 do
      m1[i][j]=-m0[i][j+rows]
    end
    v[i]=m0[i][cols]
  end
  for j=1,cols-rows-1 do
    m1[rows+j][j]=1
  end
  return m1,v,cols-1,cols-rows-1
end
function trans(matrix,rows,cols)
  local m=makeM(cols,rows)
  local i
  local j
  for i=1,rows do
    for j=1,cols do
      m[j][i]=matrix[i][j]
    end
  end
  return m
end
function mmult(m0,rows,diag,m1,asdf,cols)         --rows is the number of rows on the left matrix, cols is the number of columns on the right, and diag=asdf is the number of cols on left and rows on right
  local m=makeM(rows,cols)
  local i
  local j
  local d
  for i=1,rows do
    for j=1,cols do
      for d=1,diag do
        m[i][j]=m[i][j]+m0[i][d]*m1[d][j]
      end
    end
  end
  return m
end
function minv(matrix,size)               --this requires a square matrix, so only one dimension is necessary
  local m0=makeM(size,2*size)
  local m1=makeM(size,size)
  local i
  local j
  for i=1,size do
    for j=1,size do
      m0[i][j]=matrix[i][j]
    end
  end
  for i=1,size do
    m0[i][size+i]=1
  end
  rref(m0,size,2*size)
  for i=1,size do
    for j=1,size do
      m1[i][j]=m0[i][size+j]
    end
  end
  return m1
end
function mvmult(matrix,rows,cols,vector)
  local v=makeV(rows)
  local i
  local j
  for i=1,rows do
    for j=1,cols do
      v[i]=v[i]+matrix[i][j]*vector[j]
    end
  end
  return v
end
function vadd(v1, v2, size)
  local v=makeV(size)
  local i
  local j
  for i=1,size do
    v[i]=v1[i]+v2[i]
  end
  return v
end
function mout(matrix, rows, cols, pos)
  local i
  local j
  for i=1,rows do
  local txt=""
  for j=1,cols do
  txt=txt..(matrix[i][j])..",  "
  end
  out(i+pos,txt)
  end
end
function vminus(vector,size)
  local v = {}
  local i
  for i=1,size do
    table.insert(v,-vector[i])
  end
  return v
end
function cross(v1,v2)
  local v={0,0,0}
  v[1]=v1[2]*v2[3]-v1[3]*v2[2]
  v[2]=v1[3]*v2[1]-v1[1]*v2[3]
  v[3]=v1[1]*v2[2]-v1[2]*v2[1]
  return(v)
end
function madd(M1,M2,rows,cols)
  local M = makeM(rows,cols)
  local i
  local j
  for i=1,rows do
    for j=1,cols do
      M[i][j]=M1[i][j]+M2[i][j]
    end
  end
  return M
end
function mrot(x,y,z)
  local Z = {{math.cos(z),-math.sin(z), 0},
            {math.sin(z), math.cos(z), 0},
            {          0,          0, 1}}

  local Y = {{ math.cos(y), 0,math.sin(y)},
            {          0, 1,          0},
            {-math.sin(y), 0,math.cos(y)}}

  local X = {{1,          0,          0},
            {0, math.cos(x),-math.sin(x)},
            {0, math.sin(x), math.cos(x)}}

  return mmult(Z,3,3,mmult(Y,3,3,X,3,3),3,3)
end
function diag(vector,size)
  local D = makeM(size,size)
  local i
  for i=1,size do
    D[i][i] = vector[i]
  end
  return D
end

quaternion.lua

Code:

function qmult(q1,q2)
  local q3={0,0,0,0}
  q3[1]=q1[2]*q2[3]-q1[3]*q2[2]+q1[4]*q2[1]+q1[1]*q2[4]
  q3[2]=q1[3]*q2[1]-q1[1]*q2[3]+q1[4]*q2[2]+q1[2]*q2[4]
  q3[3]=q1[1]*q2[2]-q1[2]*q2[1]+q1[4]*q2[3]+q1[3]*q2[4]
  q3[4]=q1[4]*q2[4]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3]
  return(q3)
end
function qinv(q)
  return({-q[1],-q[2],-q[3],q[4]})
end
function rot(v,q)
  return qmult(qmult(q,v),qinv(q))
end
function q2vector(q)
  local v = makeV(3)
  local alpha12 = math.acos(q[4])   --1/2 alpha
  local scale = math.sin(alpha12)*mod(2*alpha12)
  v[1] = q[1]*scale
  v[2] = q[2]*scale
  v[3] = q[3]*scale
  return v
end

Another engineering major on the forums.

I just noticed that you need a WIDTH arguement (or rows, columns, diagonal, etc.) to most of your functions in matrix.lua, is there any reason why you didn't use table.len instead?

Also, incase you don't know about metatables, it would be possible to adjust your code slightly so that you can use regular operators (like + * - ^ etc., and equality operators) with the matrices, rather than functions like vadd(), vminus(), mmult(), etc.

I didn't know about either. I'll eventually get around to updating those.

EDIT: My mistake, it's actually table.getn()