physics.cpp

#include "main.h"
#include "geometry.h"
#include "physics.h"
#include <algorithm>

class RigidBody;
struct StructureDefinition;



FElement::FElement(double verticesLoc[][3], double matE, double matV, int inds[], int numNodesGiven){
    numNodes = numNodesGiven;
    for (int i = 0; i < numNodes; i++){
        verts.push_back(glm::vec3());
        for (int j = 0; j < 3; j++){
            verts[i][j] = verticesLoc[i][j];
        }
    }
    for (int i = 0; i < numNodes; i++){
        vertIndex.push_back(inds[i]);
    }
    E = matE;
    v = matV;
    switch (numNodes){
    case 4:
        calcElementStiffnessMatrixTet();
        break;
    case 8:
        calcElementStiffnessMatrixHex();
        break;
    default:
        break;
    }
}

FElement::~FElement(){
    delete elementStiffnessMatrix;
}

void FElement::calcElementStiffnessMatrixTet(){
    // idea for how to do it is based of  https://www.unm.edu/~bgreen/ME360/2D%20Triangular%20Elements.pdf  a method for finding stiffness matrix of a triangle in 2d
    // i expanded it into 3d and 3d element and implemented
    // calculate the element stiffness matrix for a solid tetrahedron in 3d

    // i is a direction j and k are other directions all perpendiular

    // calculate stress matrix from generalised hookes law
    // ex = sx/E -v*sy/E -v*sz/E
    // ey = -v*sx/E +sy/E -v*sz/E
    // ez = -v*sx/E -v*sy/E +sz/E
    // ei (for i in x,y,z) usually shown epsilon subscript i is strain in the direction of i
    // si (for i in x,y,z) usually shown sigma subscript i is stress in the direction of i
    // E is youngs modulus a constant in a material equal to stress/strain in 1d eg stretch a wire
    // v is poisson ratio usually shown as nu is a constant in a material equal to the ratio a cube decreases in size perpendiular to force to size increase parallel to force

    // sx = E*ex + v*sy + v*sz
    // sy = E*ey + v*sz + v*sx
    // sz = E*ez + v*sx + v*sy

    // sx = E*ex + v*E*ez + (v^2)*sx + (v^2)*sy + v*sy
    // sy = E*ey + v*(E*ez + v*sx + v*sy) + v*sx
    // sy - (v^2)*sy = E*ey +v*E*ez + (v^2)*sx + v*sx
    // sy = (1/(1-(v^2))*(E*ey + v*E*ez + (v^2 + v)*sx)

    // sx = E*ex + v*E*ez + (v^2)*sx + (v^2 + v)/(1-(v^2))*(E*ey + v*E*ez + (v^2 + v)*sx)
    // sx = E*ex + v*E*ez + (v^2)*sx + (v*(1+v))/((1-v)*(1+v))*(E*ey + v*E*ez + (v^2 + v)*sx)
    // sx = E*ex + v*E*ez + (v^2)*sx + (v/(1-v))*(E*ey + v*E*ez + (v^2 + v)*sx)
    // sx - v*sx = E*ex - v*E*ex + v*E*ez - (v^2)*E*ez + (v^2)*sx - (v^3)*sx + v*E*ey + (v^2)*E*ez + (v^3)*sx + (v^2)*sx
    // sx - v*sx = E*ex - v*E*ex + v*E*ez + v*E*ey + 2*(v^2)*sx
    // sx - v*sx - 2*(v^2)*sx = E*ex - v*E*ex + v*E*ez + v*E*ey
    // sx*(1 - v - 2*(v^2)) = E*((1-v)*ex + v*ey + v*ez)
    // sx = (E/ ((1+v)*(1-2v)) )*( (1-v)*ex + v*ey + v*ez )

    // as symetric system can swap x y and z to get other equations
    // sx = (E/ ((1+v)*(1-2v)) )*( (1-v)*ex + v*ey + v*ez )
    // sy = (E/ ((1+v)*(1-2v)) )*( v*ex + (1-v)*ey + v*ez )
    // sz = (E/ ((1+v)*(1-2v)) )*( v*ex + v*ey + (1-v)*ez )

    // shear stress formulae
    // tij (for i,j in x,y,z i != j) usually denoted tau subscript ij is shear strain in the direction combination ij, tij == tji so can ignore half of them
    // gij (for i,j in x,y,z i != j) usually denoted gamma subscript ij is shear stress in the direction combination ij, gij == gji so can ignore half of them
    // G is shear modulus = E/(2*(1+v))

    // gij = tij/G
    // tij = G*gij
    // tij = E/(2*(1+v))*gij

    // s = vec6[sx,sy,sz,txy,tyz,tzx]
    // e = vec6[ex,ey,ez,gxy,gyz,gzx]
    // D is a 6x6 matrix which turns e into s
    // s = De
    // have formulae for s in linear terms of e so can just plug in values
    // first factor out (E/ ((1+v)*(1-2v))) as it makes it simpler
    /* D = ( E/((1+v)*(1-2v)) )*[[1-v,v  ,  v,0       ,0       ,0       ]
                                 [v  ,1-v,  v,0       ,0       ,0       ]
                                 [v  ,v  ,1-v,0       ,0       ,0       ]
                                 [0  ,0  ,0  ,(1-2v)/2,0       ,0       ]
                                 [0  ,0  ,0  ,0       ,(1-2v)/2,0       ]
                                 [0  ,0  ,0  ,0       ,0       ,(1-2v)/2]]*/



    // now need to find strain matrix, assume all d(something)/d(something else) is not a derivitive but is a partial derivitive
    // x,y,z are the positions in coorinates
    // u,v,w are the changes in displacement from start in directions x,y,z
    // e = jacobian vec6[du/dx, dv/dy, dw/dz, du/dy + dv/dx, dv/dz + dw/dy, dw/dx + du/dz] = vec6[ex,ey,ez,gxy,gyz,gzx]

    // there are 3 degrees of freedom per vertex and 4 vertexs so 12 dof
    // these are displacements of vertices from starting location of vertex defined as, q = vec12[q0,q1,q2,...,q10,q11]
    // q(3i+j) = vertex i displacement in direction j (0 = x, 1 = y, 2 = z)
    // there exists a matrix such that e = Bq

    // must be able to define e at every point in the tetrahedron so need a way to interpolate between vertices
    // a similar method to used for 2d triangle can be used spliting the volume into 4 tetrahedrons V(0 to 3)
    // split by this point being the point interpolating for
    // 4 shape functions N(0 to 3) = V(0 to 3)/ total V

    // u = N0*q0 + N1*q3 + N2*q6 + N3*q9
    // v = N0*q1 + N1*q4 + N2*q7 + N3*q10
    // w = N0*q2 + N1*q5 + N2*q8 + N3*q11

    // as N0 + N1 + N2 + N3 = 1
    // N3 = 1 -N0 -N1 -N2
    // let a = N0,b = N1,c = N2
    // N3 = 1-a-b-c
    // substitute in

    // u = a*(q0 - q9) + b*(q3 - q9) + c*(q6 - q9) + q9
    // v = a*(q4 - q10) + b*(q4 - q10) + c*(q7 - q10) + q10
    // w = a*(q5 - q11) + b*(q5 - q11) + c*(q8 - q11) + q11

    // x = a*(x0 - x3) + b*(x1 - x3) + c*(x2 - x3) + x3
    // y = a*(y0 - y3) + b*(y1 - y3) + c*(y2 - y3) + y3
    // z = a*(z0 - z3) + b*(z1 - z3) + c*(z2 - z3) + z3

    // dx/da = (x0 - x3)
    // ect for all d(i in x,y,z)/d(j in a,b,c)

    // finding for all du/d(i in x,y,z)
    // could repeat for all u v and w but as symetric after one is found can work out others easy

    // chain rule du/da = (du/dx * dx/da) + (du/dy * dy/da) + (du/dz * dz/da)
    // chain rule du/db = (du/dx * dx/db) + (du/dy * dy/db) + (du/dz * dz/db)
    // chain rule du/dc = (du/dx * dx/dc) + (du/dy * dy/dc) + (du/dz * dz/dc)
    /*vec3[du/da      [[dx/da, dy/da, dz/da]     [du/dx
          ,du/db   =   [dx/db, dy/db, dz/db]  *  ,du/dy
          ,du/dc]      [dx/dc, dy/dc, dz/dc]     ,du/dz]*/

    // known vec on left and values in matrix (A) so can work out right by premultiply by A^-1 both sides to get, vec on right = (A^-1)*vec on left

    // let ijk (for i in x,y,z and for j,k in 0,1,2,3) be (vertex j pos direction i) - (vertex k pos direction i) e.g., x12 = vert1 x val - vert2 x val
    // can input values obtained from functions of x and a,b,c by differentiating x with respect to a,b,c
    /*      [[x03,y03,z03]
        A =  [x13,y13,z13]
             [x23,y23,z23]]*/
    // find A^-1
    /*                    [[y13*z23-y23*z13   , -(y03*z23-y23*z03), y03*z13-y13*z03   ]
        A^-1 = (1/detA) *  [-(x13*z23-x23*z13),  x03*z23-x23*z03  , -(x03*z13-x13*z03)]
                           [x13*y23-x23*y13   , -(x03*y23-x23*y03), x03*y13-x13*y03   ]]*/
    // volume = 1/6 * detA so we can ignore the 1/detA and hope it cancels latter
    // get rid of -ve brackets
    /*                    [[y13*z23-y23*z13 , z03*y23-z23*y03 , y03*z13-y13*z03 ]
        A^-1 = (1/detA) *  [z13*x23-z23*x13 , x03*z23-x23*z03 , z03*x13-z13*x03 ]
                           [x13*y23-x23*y13 , y03*x23-y23*x03 , x03*y13-x13*y03 ]]*/
    // multiply A^-1 by left vec3 to find du/dx
    // du/dx = (y13*z23-y23*z13)*du/da + (z03*y23-z23*y03)*du/db , (y03*z13-y13*z03)*du/dc
    // substute in du/d(a b or c)
    // du/dx = (y13*z23-y23*z13)*(q0 - q9) + (z03*y23-z23*y03)*(q3 - q9) , (y03*z13-y13*z03)*(q6 - q9)
    // expand out first bracket in each term eg bracket one is
    // ((y1-y3)*(z2-z3)-(y2-y3)*(z1-z3)) = ((y1*z2 - y1*z3 - y3*z2 + y3*z3) - (y2*z1 - y2*z3 - y3*z1 + y3*z3))
    // prev line = (y1*z2 - y2*z1 + y2*z3 - y3*z2 + y3*z1 - y1*z3)
    // this looks like a cross product of y and z then add up all terms but does not matter if getting a computer to do it as same result

    // if you expand all first brackets you get
    /* du/dx = 1/det(A) * ((y1*z2 - y2*z1 + y2*z3 - y3*z2 + y3*z1 - y1*z3)(q0 - q9)+
                           (y2*z0 - y0*z2 + y0*z3 - y3*z0 + y3*z2 - y2*z3)(q3 - q9)+
                           (y0*z1 - y1*z0 + y1*z3 - y3*z1 + y3*z0 - y0*z3)(q6 - q9))*/
    // terms 3 and 4 in each line are the same but negitive of terms 5 and 6 in a different line so when multiplying out qs only first line remains

    /* du/dx = 1/det(A) * ((y1*z2 - y2*z1 + y2*z3 - y3*z2 + y3*z1 - y1*z3)q0 +
                           (y2*z0 - y0*z2 + y0*z3 - y3*z0 + y3*z2 - y2*z3)q3 +
                           (y0*z1 - y1*z0 + y1*z3 - y3*z1 + y3*z0 - y0*z3)q6 -
                           (y1*z2 - y2*z1 + y2*z0 - y0*z2 + y0*z1 - y1*z0)q9)   */
    // get rid of negitve sign at end of line 3
    /* du/dx = 1/det(A) * ((y1*z2 - y2*z1 + y2*z3 - y3*z2 + y3*z1 - y1*z3)q0 +
                           (y2*z0 - y0*z2 + y0*z3 - y3*z0 + y3*z2 - y2*z3)q3 +
                           (y0*z1 - y1*z0 + y1*z3 - y3*z1 + y3*z0 - y0*z3)q6 +
                           (y2*z1 - y1*z2 + y0*z2 - y2*z0 + y1*z0 - y0*z1)q9)   */
    // pattern to make easy to iterate just moving around terms
    /* du/dx = 1/det(A) * ((  y2*z3 - y3*z2 + y1*z2 - y2*z1 + y3*z1 - y1*z3)q0 +
                           (- y3*z0 + y0*z3 - y2*z3 + y3*z2 - y0*z2 + y2*z0)q3 +
                           (  y0*z1 - y1*z0 + y3*z0 - y0*z3 + y1*z3 - y3*z1)q6 +
                           (- y1*z2 + y2*z1 - y0*z1 + y1*z0 - y2*z0 + y0*z2)q9)   */
    // bring out -1 from line 2 and 4
    /* du/dx = 1/det(A) * ((y2*z3 - y3*z2 + y1*z2 - y2*z1 + y3*z1 - y1*z3)q0 -
                           (y3*z0 - y0*z3 + y2*z3 - y3*z2 + y0*z2 - y2*z0)q3 +
                           (y0*z1 - y1*z0 + y3*z0 - y0*z3 + y1*z3 - y3*z1)q6 -
                           (y1*z2 - y2*z1 + y0*z1 - y1*z0 + y2*z0 - y0*z2)q9)   */




    // now have du/dx in linear terms of q
    // now need d(i in u,v,w)/d(j in x,y,z) except for combination du/dx
    // could use same method but as it is symetric problem to get u,v,w just have to swap out qs for q(s+ 1 or 2)
    // also as symetric problem to get x,y or z just have to swap out all ys and zs for xs (and either y or z which isnt being calculated)

    // now got all d(i in u,v,w)/d(j in x,y,z)

    // e = jacobian vec6[du/dx, dv/dy, dw/dz, du/dy + dv/dx, dv/dz + dw/dy, dw/dx + du/dz] = vec6[ex,ey,ez,gxy,gyz,gzx]
    // there exists a 6x12 matrix such that e = Bq
    // can just input coefficients
    // we have a strain displacment matrix (however it is 6x12 and very messy coefficents so i will not write it out here as the program will assemble it)


    // after this all the integration (put in quote marks) is very similar to how they found for triangle in 2d except there is no thickness t but as is constant could ignore already

    // "potential energy due to external forces
    // U = 1/2 * inetgral over V of ((transpose of strain matrix)  * stress matrix   dV)
    // stress matrix = D*e where we calculated D and e is strain matrix
    // U = 1/2 * inetgral over V of ((transpose of e)  * D*e   dV)
    // e = Bq
    // U = 1/2 * inetgral over V of ((transpose of B*q)  * D*B*q   dV)
    // U = 1/2 * inetgral over V of ((transpose of q)*(transpose of B) * D*B*q   dV)
    // D is constant (for same material) and B is constant for the same tetrahedron so can take out of integral
    // U = 1/2 *(transpose of q)*(transpose of B) * D*B * inetgral over V of ( dV)*q
    // integral of V over dv is the volume
    // U = 1/2 *(transpose of q)*(transpose of B) * D*B * Volume *q
    // therefore stiffness matrix of tetrahedron k = V*((transpose of B)*D*B)
    // so stiffness matrix of whole thing is sum of k for each element"

    // detA must be computed i think
    /*      [[x03,y03,z03]
        A =  [x13,y13,z13]
             [x23,y23,z23]]*/
    // detA = x03*(y13*z23-z13*y23) +y03*(z13*x23-x13*z23) +z03*(x13*y23-y13*x23)
    // detA = [x03,y03,z03].[ y13*z23-z13*y23, z13*x23-x13*z23, x13*y23-y13*x23]
    // detA = v03.(v13 X v23) with dot and cross products (triple scaler product)

    // actual program

    elementStiffnessMatrix = new matNxN(12);



    double D[6][6] = {{1-v,v,v,0,0,0}
                     ,{v,1-v,v,0,0,0}
                     ,{v,v,1-v,0,0,0}
                     ,{0,0,0,(1-2*v)/2.0,0,0}
                     ,{0,0,0,0,(1-2*v)/2.0,0}
                     ,{0,0,0,0,0,(1-2*v)/2.0}};// constant for a material and space
    for (int i = 0; i < 6; i++){
        for (int j = 0; j < 6; j++){
            D[i][j] *= E/((1+v)*(1-2*v));
        }
    }
    //std::cout << "got D matrix\n";
    //std::cout << E << "\n";
    //std::cout << D[0][0] << "\n";

    double B[6][12] = {};//  = {} should mean dont need to initailise anymore
    /*for (int i = 0; i < 6; i++){for (int j = 0; j < 12; j++){B[i][j] = 0;}}*/
    double v0[3] = {verts[0][0]-verts[3][0],verts[0][1]-verts[3][1],verts[0][2]-verts[3][2]};
    double v1[3] = {verts[1][0]-verts[3][0],verts[1][1]-verts[3][1],verts[1][2]-verts[3][2]};
    double v2[3] = {verts[2][0]-verts[3][0],verts[2][1]-verts[3][1],verts[2][2]-verts[3][2]};
    double detA = 0;
    for (int i = 0; i < 3; i++){
        detA += v0[i] * (v1[(i+1)%3]*v2[(i+2)%3] - v1[(i+2)%3]*v2[(i+1)%3]);
    }
    detA = fabs(detA);// dont know if got right way round so may be negitive


    for (int i = 0; i < 3; i++){// for i in u,v,w
        for (int j = 0; j < 3; j++){// for j in x,y,z
            int row = 0;
            int offset = i;
            if (i == j){// du/dx, dv/dy, dw/dz
                row = i;
            }
            else{
                // if i + j == 1 (du/dy or dv/dx) row = 3
                // if i + j == 2 (du/dz or dw/dx (cant be dv/dy as already checked i != j)) row = 5
                // if i + j == 3 (dv/dz or dw/dy) row = 4
                switch (i+j){
                case 1:row = 3;break;
                case 2:row = 5;break;
                case 3:row = 4;break;
                default:break;}
            }
            for (int k = 0; k < 4; k++){// for k in some q (every third with an offset)
                int a = (j+1)%3; int b = (j+2)%3;
                double val = verts[(2+k)%4][a]*verts[(3+k)%4][b] - verts[(3+k)%4][a]*verts[(2+k)%4][b]+
                             verts[(1+k)%4][a]*verts[(2+k)%4][b] - verts[(2+k)%4][a]*verts[(1+k)%4][b]+
                             verts[(3+k)%4][a]*verts[(1+k)%4][b] - verts[(1+k)%4][a]*verts[(3+k)%4][b];//(y2*z3 - y3*z2 + y1*z2 - y2*z1 + y3*z1 - y1*z3)
                val *= (((k+1)%2)*2)-1;// times 1 if k is even; times -1 if k is odd

                B[row][offset + 3*k] = val/detA;
            }
        }
    }
    //std::cout << "got B matrix\n";
    //std::cout << B[0][0] << "\n";

    // k = transpose(B)*D*B
    double DB[6][12];
    for (int i = 0; i < 6; i++){
        for (int j = 0; j < 12; j++){
            DB[i][j] = 0;
            for (int k = 0; k < 6; k++){
                DB[i][j] += D[i][k]*B[k][j];
            }
        }
    }
    //double elementK[12][12];
    for (int i = 0; i < 12 ; i++){
        for (int j = 0; j < 12; j++){
            elementStiffnessMatrix->val[i][j] = 0;
            for (int k = 0; k < 6; k++){
                //k[i][j] += transpose(B[i][k])*DB[k][j]; // transpose reverses index order
                elementStiffnessMatrix->val[i][j] += B[k][i]*DB[k][j];
            }
        }
    }

    for (int i = 0; i < 12; i++){
        for (int j = 0; j < 12; j++){
            //std::cout << elementStiffnessMatrix.val[i][j] << "\t";
        }
        //std::cout << "\n";
    }
    //std::cout << "got K matrix\n";
}

void FElement::calcElementStiffnessMatrixHex(){
    // used for hexahedral elements
    // D matrix is the same for all 3d solid elements so use same as the tetrahedral version
    // http://nliebeaux.free.fr/ressources/introfem.pdf used to help find strain matrix

    // now need to find strain matrix, assume all d(something)/d(something else) is not a derivitive but is a partial derivitive
    // x,y,z are the positions in coorinates
    // u,v,w are the changes in displacement from start in directions x,y,z
    // e = jacobian vec6[du/dx, dv/dy, dw/dz, du/dy + dv/dx, dv/dz + dw/dy, dw/dx + du/dz] = vec6[ex,ey,ez,gxy,gyz,gzx]

    // there are 3 degrees of freedom per vertex and 8 vertexs so 24 dof
    // these are displacements of vertices from starting location of vertex defined as, q = vec24[q0,q1,q2,...,q10,q11]
    // q(3i+j) = vertex i displacement in direction j (0 = x, 1 = y, 2 = z)
    // there exists a matrix such that e = Bq

    // must be able to define e at every point in the hexahedron so need a way to interpolate between vertices
    // need shape functions which shape function is 1 for 1 vertex and 0 for all others
    // then get new pos using transformed pos = sum for all shape functions(Nx(original pos))
    // chosen function Ni(ξ,η,ζ) = 0.125 * (1 + ξi * ξ) (1 + ηi * η) (1 + ζi * ζ)
    // each corner will have ξηζ values = plus or minus 1
    // so each point has transformed pos ξηζ in range [-1,1]^3 where point i is position (ξi,ηi,ζi)

    // B = [B0 B1 B2 ... B7]  the strain matrix
    // Bi = [dNi/dx,    0 ,     0]
    //      [0     ,dNi/dy,     0]
    //      [0     ,    0 ,dNi/dz]
    //      [dNi/dy,dNi/dx,     0]
    //      [0     ,dNi/dz,dNi/dy]
    //      [dNi/dz,    0 ,dNi/dx]

    // dx/dξ = sum(0 to 7) (dNi/dξ * xi)
    // can be used to find jacobian below

    //          [dx/dξ,dy/dξ,dz/dξ]
    // let J =  [dx/dη,dy/dη,dz/dζ]
    //          [dx/dζ,dy/dη,dz/dζ]

    // jacobian *vector =  this by standard matrix multiplication = final vector through chain rule
    //     [dNi/dx]   [(dNi/dx)*(dx/dξ) + (dNi/dy)*(dy/dξ) + (dNi/dz)*(dz/dξ)]   [dNi/dξ]
    // J * [dNi/dy] = [(dNi/dx)*(dx/dη) + (dNi/dy)*(dy/dη) + (dNi/dz)*(dz/dη)] = [dNi/dη]
    //     [dNi/dz]   [(dNi/dx)*(dx/dζ) + (dNi/dy)*(dy/dζ) + (dNi/dz)*(dz/dζ)]   [dNi/dζ]

    // premultiply by inverse of J
    // [dNi/dx]            [dNi/dξ]
    // [dNi/dy] = [J]^-1 * [dNi/dη]
    // [dNi/dz]            [dNi/dζ]

    // this gives vars needed to make strain matrix

    // dNi/dξ = d(0.125 * (1 + ξi * ξ) (1 + ηi * η) (1 + ζi * ζ))/dξ = (0.125 * ξi * (1 + ηi * η) (1 + ζi * ζ))
    // dNi/d(ξ,η,ζ) can be calculated at any point in element given (ξ,η,ζ)

    // stiffness matrix = volume integral of ( transpose of (B(ξ,η,ζ))  *  elastistity matrix  *  B(ξ,η,ζ))dv
    // dv = dxdydz = |J| * dξdηdζ
    // then change ranges to local coordinates (ξ,η,ζ) and take the definite triple integral in range [-1,1] for all of (ξ,η,ζ)

    // definite integrals in the range [-1,1] can be solved using gaussian quadrature which turns it into the sum of values at specific points times by weights
    // turns integral [-1,1] of (f(x))dx into sum (from 1 to n) of (f(point n)*(weight n))
    // for linear polynomials use of 2 points (per integral which as 3d means 2^3 points) is recommended by the paper
    // the points for 2point gaussian quatrature (using lagrange polynomials) is (1/root(3) and -1/root(3)) with weights (1 and 1)
    // so for the 3d one points are x = +or- 1/root(3)     y = +or- 1/root(3)     z = +or- 1/root(3)
    // all with weight 1 (as have a *0.125 term in shape function will end up with total weight 1)

    // dNi/dξ at point(ξ0,η0,ζ0) = 0.125 * (ξi * ξ0) (1 + ηi * η0) (1 + ζi * ζ0)

    elementStiffnessMatrix = new matNxN(24);


    double D[6][6] = {{1-v,v,v,0,0,0}
                     ,{v,1-v,v,0,0,0}
                     ,{v,v,1-v,0,0,0}
                     ,{0,0,0,(1-2*v)/2.0,0,0}
                     ,{0,0,0,0,(1-2*v)/2.0,0}
                     ,{0,0,0,0,0,(1-2*v)/2.0}};// constant for a material and space
    for (int i = 0; i < 6; i++){
        for (int j = 0; j < 6; j++){
            D[i][j] *= E/((1+v)*(1-2*v));
        }
    }

    const double points[2] = {-sqrt(1.0/3.0),sqrt(1.0/3.0)};

    for (int i = 0; i < 2; i++){
        for (int j = 0; j < 2; j++){
            for (int k = 0; k < 2; k++){
                double point[3] = {points[i],points[j],points[k]};// location of test point in transformed space
                double B[24][6];
                double dNiBYdj[8][3];// rate of change of shape function at point given
                for (int l = 0; l < 8; l++){
                    int NPoint[3] = {2*(l/4)-1 , 2*((l/2) %2) -1, 2*(l%2) -1};// location of vertex in transformed space
                    dNiBYdj[l][0] = 0.125 * (NPoint[0] * point[0]) * (1 + NPoint[1] * point[1]) * (1 + NPoint[2] * point[2]);
                    dNiBYdj[l][1] = 0.125 * (1 + NPoint[0] * point[0]) * (NPoint[1] * point[1]) * (1 + NPoint[2] * point[2]);
                    dNiBYdj[l][2] = 0.125 * (1 + NPoint[0] * point[0]) * (1 + NPoint[1] * point[1]) * (NPoint[2] * point[2]);
                }

                glm::mat3x3 diBYdj;// dx/d(local space) the jacobi matrix
                for (int l = 0; l < 3; l++){
                    for (int m = 0; m < 3; m++){
                        diBYdj[l][m] = 0;
                        for (int n = 0; n < 8; n++){
                            diBYdj[l][m] += (dNiBYdj[n][m]) * verts[n][l];
                        }
                    }
                }

                glm::mat3x3 invJacobi = glm::inverse(diBYdj);
                double detJacobi = glm::determinant(diBYdj);
                for (int l = 0; l < 8; l++){
                    glm::vec3 dNiBydGlobal = invJacobi * glm::vec3(dNiBYdj[l][0],dNiBYdj[l][1],dNiBYdj[l][2]);// dNi/dx and dNi/dy and dNi/dz
                    B[3*l + 0][0] = dNiBydGlobal[0];
                    B[3*l + 1][1] = dNiBydGlobal[1];
                    B[3*l + 2][2] = dNiBydGlobal[2];

                    B[3*l + 0][3] = dNiBydGlobal[1];
                    B[3*l + 0][5] = dNiBydGlobal[2];
                    B[3*l + 1][3] = dNiBydGlobal[0];
                    B[3*l + 1][4] = dNiBydGlobal[2];
                    B[3*l + 2][4] = dNiBydGlobal[1];
                    B[3*l + 2][5] = dNiBydGlobal[0];

                    B[3*l + 0][4] = 0;
                    B[3*l + 1][5] = 0;
                    B[3*l + 2][3] = 0;

                    B[3*l + 0][1] = 0;
                    B[3*l + 0][2] = 0;
                    B[3*l + 1][0] = 0;
                    B[3*l + 1][2] = 0;
                    B[3*l + 2][0] = 0;
                    B[3*l + 2][1] = 0;

                    //std::cout << dNiBydGlobal[0] << " " << dNiBydGlobal[1] << " " << dNiBydGlobal[2] << "\n";
                }// assemble the B matrix
                /*for (int l = 0; l < 6; l++){
                    for (int m = 0; m < 24; m++){
                        std::cout << std::round(B[m][l]*1000)/1000 << "\t";
                    }
                    std::cout << "\n";
                }
                std::cout << "\n";*/

                // k = transpose(B)*D*B
                double DB[6][24];
                for (int l = 0; l < 6; l++){
                    for (int m = 0; m < 24; m++){
                        DB[l][m] = 0;
                        for (int n = 0; n < 6; n++){
                            DB[l][m] += D[l][n]*B[m][n];// B indexing was backward
                        }
                        //std::cout << std::round(DB[l][m]*1000)/1000 << "\t";
                    }
                    //std::cout << "\n";
                }
                //std::cout << "\n";
                //double elementK[24][24];
                for (int l = 0; l < 24 ; l++){
                    for (int m = 0; m < 24; m++){
                        double delta = 0;
                        for (int n = 0; n < 6; n++){
                            //k[l][m] += transpose(B[l][n])*DB[n][m]; // transpose reverses index order except reversing happens twice as data structure must also be reversed
                            delta += B[l][n]*DB[n][m];
                        }
                        elementStiffnessMatrix->val[l][m] += delta * fabs(detJacobi);// as dv = |J| * dξdηdζ need to times by det of the jacobi matrix
                        //std::cout << std::round(delta) << "\t";
                    }
                    //std::cout << "\n";
                }
                //std::cout << "\n";
                //std::cout << detJacobi << "\n";
            }
        }
    }
    /*for (int l = 0; l < 24; l++){
        for (int m = 0; m < 24; m++){
            std::cout << std::round(elementStiffnessMatrix->val[m][l]*1000)/1000 << "\t";
        }
        std::cout << "\n";
    }
    std::cout << "\n";*/
}

std::vector<double> FElement::calcNodalSressFromDisplacement(std::vector<double> *globalDisplacements){
    // how to find stress from https://quickfem.com/wp-content/ups/IFEM.Ch28.pdf
    // stress = D (material matrix of element) * ε (strain field)
    // ε = B (strain displacement matrix) * u (displacement known)
    // so stress = DBu

    // for tetrahedral nodes no guass points needed so I can just plug in values and get out displacements

    // for hexahedral nodes it is a bit more complicated
    // stress to be evaluated at each gauss point then extrapolated
    // I assume that the displacment is assumed to be the same ??? or is it interpolated to find val at gauss point
    // so evaluate BD at each gauss point (this was done in calculating the stiffness matrix but then discarded)
    // then just times by displacement to get stress at the gauss point
    // then extrapolate

    switch (numNodes)
    {
    case 4:// tetrahedron
        return calcNodalSressFromDisplacementTet(globalDisplacements);
        break;
    case 8:// hexahedron
        return calcNodalSressFromDisplacementHex(globalDisplacements);
        break;

    default:
        break;
    }
}

std::vector<double> FElement::calcNodalSressFromDisplacementTet(std::vector<double> *globalDisplacements){
    // get DB matrix then times by displacements to get stress
    std::vector<double> stressValues;


    double D[6][6] = {{1-v,v,v,0,0,0}
                     ,{v,1-v,v,0,0,0}
                     ,{v,v,1-v,0,0,0}
                     ,{0,0,0,(1-2*v)/2.0,0,0}
                     ,{0,0,0,0,(1-2*v)/2.0,0}
                     ,{0,0,0,0,0,(1-2*v)/2.0}};// constant for a material and space
    for (int i = 0; i < 6; i++){
        for (int j = 0; j < 6; j++){
            D[i][j] *= E/((1+v)*(1-2*v));
        }
    }

    double B[6][12] = {};//  = {} should mean dont need to initailise anymore
    /*for (int i = 0; i < 6; i++){for (int j = 0; j < 12; j++){B[i][j] = 0;}}*/
    double v0[3] = {verts[0][0]-verts[3][0],verts[0][1]-verts[3][1],verts[0][2]-verts[3][2]};
    double v1[3] = {verts[1][0]-verts[3][0],verts[1][1]-verts[3][1],verts[1][2]-verts[3][2]};
    double v2[3] = {verts[2][0]-verts[3][0],verts[2][1]-verts[3][1],verts[2][2]-verts[3][2]};
    double detA = 0;
    for (int i = 0; i < 3; i++){
        detA += v0[i] * (v1[(i+1)%3]*v2[(i+2)%3] - v1[(i+2)%3]*v2[(i+1)%3]);
    }
    detA = fabs(detA);// dont know if got right way round so may be negitive


    for (int i = 0; i < 3; i++){// for i in u,v,w
        for (int j = 0; j < 3; j++){// for j in x,y,z
            int row = 0;
            int offset = i;
            if (i == j){// du/dx, dv/dy, dw/dz
                row = i;
            }
            else{
                // if i + j == 1 (du/dy or dv/dx) row = 3
                // if i + j == 2 (du/dz or dw/dx (cant be dv/dy as already checked i != j)) row = 5
                // if i + j == 3 (dv/dz or dw/dy) row = 4
                switch (i+j){
                case 1:row = 3;break;
                case 2:row = 5;break;
                case 3:row = 4;break;
                default:break;}
            }
            for (int k = 0; k < 4; k++){// for k in some q (every third with an offset)
                int a = (j+1)%3; int b = (j+2)%3;
                double val = verts[(2+k)%4][a]*verts[(3+k)%4][b] - verts[(3+k)%4][a]*verts[(2+k)%4][b]+
                             verts[(1+k)%4][a]*verts[(2+k)%4][b] - verts[(2+k)%4][a]*verts[(1+k)%4][b]+
                             verts[(3+k)%4][a]*verts[(1+k)%4][b] - verts[(1+k)%4][a]*verts[(3+k)%4][b];//(y2*z3 - y3*z2 + y1*z2 - y2*z1 + y3*z1 - y1*z3)
                val *= (((k+1)%2)*2)-1;// times 1 if k is even; times -1 if k is odd

                B[row][offset + 3*k] = val/detA;
            }
        }
    }
    //std::cout << "got B matrix\n";

    // stress = D*B * u
    double DB[6][12];
    for (int i = 0; i < 6; i++){
        for (int j = 0; j < 12; j++){
            DB[i][j] = 0;
            for (int k = 0; k < 6; k++){
                DB[i][j] += D[i][k]*B[k][j];
            }
        }
    }

    for (int i = 0; i < 6; i++){
        stressValues.push_back(0);
        for (int j = 0; j < 12; j++){
            stressValues[i] += DB[i][j]*(*globalDisplacements)[vertIndex[i/3]*3 + i%3];
        }
    }

    for (int i = 0; i < 18; i++){
        stressValues.push_back(stressValues[i%6]);// because stress is all the same in a linear tetrahedral element just copy result for other 3 vertices
    }

    return stressValues;
}

std::vector<double> FElement::calcNodalSressFromDisplacementHex(std::vector<double> *globalDisplacements){
    // get DB matrix per gauss point
    // then times each of them by displacements to get stress at guass points
    // then extrapolate to get nodal stress
    std::vector<double> stressValues;
    std::vector<double> gaussStressValues;

    double D[6][6] = {{1-v,v,v,0,0,0}
                     ,{v,1-v,v,0,0,0}
                     ,{v,v,1-v,0,0,0}
                     ,{0,0,0,(1-2*v)/2.0,0,0}
                     ,{0,0,0,0,(1-2*v)/2.0,0}
                     ,{0,0,0,0,0,(1-2*v)/2.0}};// constant for a material and space
    for (int i = 0; i < 6; i++){
        for (int j = 0; j < 6; j++){
            D[i][j] *= E/((1+v)*(1-2*v));
        }
    }

    const double points[2] = {-sqrt(1.0/3.0),sqrt(1.0/3.0)};
    const double invPoints[2] = {-sqrt(3.0),sqrt(3.0)};// the inverse of values in points

    for (int i = 0; i < 8; i++){
        double point[3] = {points[i/4],points[(i/2)%2],points[i%2]};// location of test point in transformed space
        double B[24][6];
        double dNiBYdj[8][3];// rate of change of shape function at point given
        for (int l = 0; l < 8; l++){
            int NPoint[3] = {2*(l/4)-1 , 2*((l/2) %2) -1, 2*(l%2) -1};// location of vertex in transformed space
            dNiBYdj[l][0] = 0.125 * (NPoint[0] * point[0]) * (1 + NPoint[1] * point[1]) * (1 + NPoint[2] * point[2]);
            dNiBYdj[l][1] = 0.125 * (1 + NPoint[0] * point[0]) * (NPoint[1] * point[1]) * (1 + NPoint[2] * point[2]);
            dNiBYdj[l][2] = 0.125 * (1 + NPoint[0] * point[0]) * (1 + NPoint[1] * point[1]) * (NPoint[2] * point[2]);
        }

        glm::mat3x3 diBYdj;// dx/d(local space) the jacobi matrix
        for (int l = 0; l < 3; l++){
            for (int m = 0; m < 3; m++){
                diBYdj[l][m] = 0;
                for (int n = 0; n < 8; n++){
                    diBYdj[l][m] += (dNiBYdj[n][m]) * verts[n][l];
                }
            }
        }

        glm::mat3x3 invJacobi = glm::inverse(diBYdj);
        double detJacobi = glm::determinant(diBYdj);
        for (int l = 0; l < 8; l++){
            glm::vec3 dNiBydGlobal = invJacobi * glm::vec3(dNiBYdj[l][0],dNiBYdj[l][1],dNiBYdj[l][2]);// dNi/dx and dNi/dy and dNi/dz
            B[3*l + 0][0] = dNiBydGlobal[0];
            B[3*l + 1][1] = dNiBydGlobal[1];
            B[3*l + 2][2] = dNiBydGlobal[2];

            B[3*l + 0][3] = dNiBydGlobal[1];
            B[3*l + 0][5] = dNiBydGlobal[2];
            B[3*l + 1][3] = dNiBydGlobal[0];
            B[3*l + 1][4] = dNiBydGlobal[2];
            B[3*l + 2][4] = dNiBydGlobal[1];
            B[3*l + 2][5] = dNiBydGlobal[0];

            B[3*l + 0][4] = 0;
            B[3*l + 1][5] = 0;
            B[3*l + 2][3] = 0;

            B[3*l + 0][1] = 0;
            B[3*l + 0][2] = 0;
            B[3*l + 1][0] = 0;
            B[3*l + 1][2] = 0;
            B[3*l + 2][0] = 0;
            B[3*l + 2][1] = 0;

            //std::cout << dNiBydGlobal[0] << " " << dNiBydGlobal[1] << " " << dNiBydGlobal[2] << "\n";
        }// assemble the B matrix

        // k = transpose(B)*D*B
        double DB[6][24];
        for (int l = 0; l < 6; l++){
            for (int m = 0; m < 24; m++){
                DB[l][m] = 0;
                for (int n = 0; n < 6; n++){
                    DB[l][m] += D[l][n]*B[m][n];
                }
            }
        }

        for (int j = 0; j < 6; j++){
            gaussStressValues.push_back(0);
            for (int k = 0; k < 24; k++){
                gaussStressValues[j] += DB[j][k]*(*globalDisplacements)[vertIndex[k/3]*3 + k%3];
            }
        }
    }
    for (int i = 0; i < 48; i++){
        stressValues.push_back(0);
    }

    for (int i = 0; i < 6; i++){// for each component
        for (int j = 0; j < 8; j++){// for each node
            for (int k = 0; k < 8; k++){// for each gauss point
                // if i XORed j and k that would give me which bits are different so I could use that to check which invPoint to add
                int jXORk = j ^ k; // ^ does XOR op in c++
                stressValues[j*6+i] += 0.125*(1 + invPoints[(jXORk & 4) >> 2])*(1 + invPoints[(jXORk & 2) >> 1])*(1 + invPoints[(jXORk & 1)]) * gaussStressValues[k*6+i];
            }
        }
    }

    return stressValues;
}

FEStructure::FEStructure(Program* program, ModelAndForces &design){
    p = program;
    splitIntoElements(design);
}

FEStructure::~FEStructure(){

}

struct BigArray{
    float space[128][128][128];
};

void FEStructure::splitIntoElements(ModelAndForces &design){

    // for each cell a point must be found
    // the points in all cells surrounding a corner which is inside the region make up an element
    // if all corners in a cell are inside then the point is in the middle of the cell


    nodes.clear();

    BigArray* s = new BigArray();

    for (int i = 0; i < 128; i++){
        for (int j = 0; j < 128; j++){
            for (int k = 0; k < 128; k++){
                double x = i-64;
                double y = j-64;
                double z = k-64;

                s->space[i][j][k] = design.model.getSDF(glm::vec3(x,y,z));
            }
        }
    }



    std::unordered_map<int, uint32_t> nodeInds;
    uint32_t currentNode = 0;
    for (int i = 0; i < 127; i++){
        for (int j = 0; j < 127; j++){
            for (int k = 0; k < 127; k++){
                double x = i-64;
                double y = j-64;
                double z = k-64;

                int key = (i)*(128*128) + (j)*(128) + (k);
                int usedNodeX = ((s->space[i][j][k] <= 0.0) + (s->space[i][j][k+1] <= 0.0) + (s->space[i][j+1][k] <= 0.0) + (s->space[i][j+1][k+1] <= 0.0) +
                                (s->space[i+1][j][k] <= 0.0) + (s->space[i+1][j][k+1] <= 0.0) + (s->space[i+1][j+1][k] <= 0.0) + (s->space[i+1][j+1][k+1] <= 0.0));


                if (usedNodeX == 0){// not used in the middle of nowhere not in the model

                }
                else if (usedNodeX == 8){// in the middle of the model not a surface node
                    nodes.push_back(x+0.5);
                    nodes.push_back(y+0.5);
                    nodes.push_back(z+0.5);

                    nodeInds[key] = currentNode;
                    currentNode++;
                }
                else{// a surface node
                    std::vector<glm::vec3> midpoints;
                    std::vector<glm::vec3> mpNormals;
                    for (int l = 0; l < 12; l++){// for each edge
                        int a = l/4;
                        int b = l%4;
                        glm::ivec3 v = glm::ivec3(0,b%2,(b/2)%2);
                        glm::ivec3 v1 = glm::ivec3(v[a],v[(2+a)%3],v[(1+a)%3]);
                        glm::ivec3 v2 = glm::ivec3(v1.x+(a == 0),v1.y+(a == 1),v1.z+(a == 2));


                        if ((s->space[i+ v1.x][j+ v1.y][k+ v1.z] <= 0.0) != (s->space[i+ v2.x][j+ v2.y][k+ v2.z] <= 0.0)){// if the end points dont match
                            //midpoints.push_back(glm::vec3(v1+v2)/glm::vec3(2.0));
                            midpoints.push_back((glm::vec3(v2)*glm::vec3(s->space[i+ v1.x][j+ v1.y][k+ v1.z]) -
                                                glm::vec3(v1)*glm::vec3(s->space[i+ v2.x][j+ v2.y][k+ v2.z]))
                                                /glm::vec3(s->space[i+ v1.x][j+ v1.y][k+ v1.z] - s->space[i+ v2.x][j+ v2.y][k+ v2.z]));

                            //std::cout << v1[0] << " " << v1[1] << " " << v1[2] << "\n";
                            //std::cout << v2[0] << " " << v2[1] << " " << v2[2] << "\n";
                            //std::cout << midpoints[midpoints.size()-1][0] << " " << midpoints[midpoints.size()-1][1] << " " << midpoints[midpoints.size()-1][2] << "\n";

                            //std::cout << s->space[i+ v1.x][j+ v1.y][k+ v1.z] << " " << s->space[i+ v2.x][j+ v2.y][k+ v2.z] << "\n";
                            //std::cout << design.getSDF(glm::vec3(x,y,z)+glm::vec3(v1+v2)/glm::vec3(2.0)) << "\n";
                            //std::cout << design.getSDF(glm::vec3(x,y,z)+midpoints[midpoints.size()-1]) << "\n\n";

                            mpNormals.push_back(design.model.getNormal(glm::vec3(x,y,z)+midpoints[midpoints.size()-1]));
                            //std::cout << midpoints[midpoints.size()-1][0] << " " << midpoints[midpoints.size()-1][1] << " " << midpoints[midpoints.size()-1][2] << "\n";
                            //std::cout << s->space[i+ v1.x][j+ v1.y][k+ v1.z] << " " << s->space[i+ v2.x][j+ v2.y][k+ v2.z] << "\n";
                        }
                    }

                    glm::vec3 result = glm::vec3(0.0);
                    if (midpoints.size() != 0){


                        for (int l = 0; l < midpoints.size(); l++){
                            result += midpoints[l];
                        }
                        result /= midpoints.size();

                        /*glm::vec3 step = glm::vec3(0.0);// not great at optimising so ignored for now
                        for (int l = 0; l < midpoints.size(); l++){
                            float dist = glm::dot(result,mpNormals[l])-glm::dot(midpoints[l],mpNormals[l]);
                            // assess how close to the plane which a line through the result and midpoint would give the correct normal
                            result -= glm::vec3(dist*0.7)*mpNormals[l];
                        }

                        */float error = 0;// assess how good at matching normals it is

                        for (int l = 0; l < midpoints.size(); l++){
                            error += fabs(glm::dot(result,mpNormals[l])-glm::dot(midpoints[l],mpNormals[l]));
                            // assess how close to the plane which a line through the result and midpoint would give the correct normal
                        }
                        //std::cout << error << "\n";
                    }

                    nodes.push_back(x+result[0]);// only slightly optimised
                    nodes.push_back(y+result[1]);
                    nodes.push_back(z+result[2]);

                    surfaceNodes[currentNode] = static_cast<uint16_t>(surfaceNodes.size());
                    //std::cout << currentNode << "\n";

                    nodeInds[key] = currentNode;
                    currentNode++;

                }
            }
        }
    }
    std::cout << currentNode << " num nodes\n";

    std::cout << surfaceNodes.size() << " num surface nodes\n";

    numNodes = nodes.size()/3;



    for (int i = 1; i < 127; i++){
        for (int j = 1; j < 127; j++){
            for (int k = 1; k < 127; k++){
                if (s->space[i][j][k] > 0.0){
                    continue;
                }

                int index[8];
                for (int l = 0; l < 8; l++){
                    int key = (i+(l/4)-1)*(128*128) + (j+((l/2)%2)-1)*(128) + (k+(l%2)-1);
                    index[l] = nodeInds[key];
                }

                // the vector below is just to quickly change the order of vertices in an element if there was an error
                const std::vector<int> elementNodes = {0,1,2,3,4,5,6,7};

                double verts[8][3];
                for (int l = 0; l < 8; l++){
                    for (int m = 0; m < 3; m++){
                        verts[l][m] = nodes[index[elementNodes[l]]*3 + m];
                    }
                }
                int inds[8];
                for (int l = 0; l < 8; l++){
                    inds[l] = index[elementNodes[l]];
                }
                elements.push_back(new FElement(verts, 10000, 0.4, inds, 8));
            }
        }
    }

    delete s;
}

void FEStructure::getGlobalStiffnessMatrix(){
    globalStiffnessMatrix = new double*[numNodes*3];
    for (int i = 0; i < numNodes*3; i++){
        globalStiffnessMatrix[i] = new double[numNodes*3];
    }

    for (int i = 0; i < numNodes*3; i++){
        for (int j = 0; j < numNodes*3; j++){
            globalStiffnessMatrix[i][j] = 0;
        }
    }
    for (FElement* e: elements){
        matNxN* elementStressMat = e->getElementStiffnessMatrix();
        std::vector<uint32_t> indices = e->getVertIndices();
        for (int i = 0; i < elementStressMat->size; i++){
            for (int j = 0; j < elementStressMat->size; j++){
                globalStiffnessMatrix[(indices[i/3])*3 + i%3][(indices[j/3])*3 + j%3] += elementStressMat->val[i][j];
            }
        }
    }
}

void FEStructure::getSparseGlobalStiffnessMatrix(){// for when space becomes an issue

    sparseGlobalStiffnessMatrix.valInds.clear();
    sparseGlobalStiffnessMatrix.values.clear();
    for (int i = 0; i < 3*numNodes; i++){
        std::vector<int> newInds;
        std::vector<double> newVals;
        sparseGlobalStiffnessMatrix.valInds.push_back(newInds);
        sparseGlobalStiffnessMatrix.values.push_back(newVals);
    }

    for (FElement* e: elements){
        matNxN* elementStressMat = e->getElementStiffnessMatrix();
        std::vector<uint32_t> indices = e->getVertIndices();
        for (int i = 0; i < elementStressMat->size; i++){
            for (int j = 0; j < elementStressMat->size; j++){
                // because it is small (0-20) linear search is probably fast enough also means dont need to sort
                int x = (indices[i/3])*3 + i%3;
                int y = (indices[j/3])*3 + j%3;
                int ind = -1;
                for (int k = 0; k < sparseGlobalStiffnessMatrix.valInds[y].size(); k++){
                    if (sparseGlobalStiffnessMatrix.valInds[y][k] == x){
                        ind = k;
                    }
                }
                if (ind == -1){
                    sparseGlobalStiffnessMatrix.values[y].push_back(0);
                    sparseGlobalStiffnessMatrix.valInds[y].push_back(x);
                    ind = sparseGlobalStiffnessMatrix.valInds[y].size()-1;
                }
                sparseGlobalStiffnessMatrix.values[y][ind] += elementStressMat->val[i][j];
                //globalStiffnessMatrix[(indices[i/3])*3 + i%3][(indices[j/3])*3 + j%3] += elementStressMat.val[i][j];
            }
        }
    }

    // the fact it is symetric makes removing nodes easier as know where things which need to be removed are
}


void FEStructure::makeSystemOfLinearEquations(std::vector<bool> dDefined, std::vector<double> forces, std::vector<double> displacements){
    linearEquations.values.clear();
    linearEquations.index.clear();

    for (int i = 0; i < 3*numNodes; i++){
        if (!dDefined[i]){// unknown displacement
            std::vector<double> newRow;
            double val = forces[i];
            for (int j = 0; j < 3*numNodes; j++){
                if (dDefined[j]){
                    val -= globalStiffnessMatrix[i][j]*displacements[j];
                }
                else{
                    newRow.push_back(globalStiffnessMatrix[i][j]);
                }
            }
            newRow.push_back(val);

            linearEquations.values.push_back(newRow);
            //index.push_back(index.size());// for printing in order not used any more
            linearEquations.index.push_back(i);
        }
    }
    linearEquations.width = linearEquations.values[0].size();
    linearEquations.height = linearEquations.values.size();
    std::cout << "set up\n";
    std::cout << linearEquations.width << " " << linearEquations.height << "\n";
}

void FEStructure::makeSparseSystemOfLinearEquations(std::vector<bool> dDefined, std::vector<double> forces, std::vector<double> displacements){
    sparseLinearEquations.values.clear();
    sparseLinearEquations.index.clear();
    sparseLinearEquations.valInds.clear();
    sparseLinearEquations.result.clear();

    for (int i = 0; i < 3*numNodes; i++){
        if (!dDefined[i]){// unknown displacement
            std::vector<double> newRow;
            std::vector<int> newInds;
            double val = forces[i];
            int ind = 0;
            for (int j = 0; j < 3*numNodes; j++){
                if (dDefined[j]){
                    val -= globalStiffnessMatrix[i][j]*displacements[j];
                }
                else{
                    if (fabs(globalStiffnessMatrix[i][j]) > 0.0000001){
                        newRow.push_back(globalStiffnessMatrix[i][j]);
                        newInds.push_back(ind);
                        if (i == j){// on the diagonal
                            sparseLinearEquations.diagonalInd.push_back(newInds.size()-1);
                        }
                    }
                    ind++;
                }
            }
            if (sparseLinearEquations.diagonalInd.size() != sparseLinearEquations.result.size()+1){
                sparseLinearEquations.diagonalInd.push_back(-1);
            }
            sparseLinearEquations.result.push_back(val);

            sparseLinearEquations.values.push_back(newRow);
            sparseLinearEquations.valInds.push_back(newInds);
            sparseLinearEquations.index.push_back(i);
        }
    }
    sparseLinearEquations.height = sparseLinearEquations.values.size();
    std::cout << "set up\n";
    std::cout << sparseLinearEquations.height << "\n";
}

void FEStructure::makeSparseSystemOfLinearEquationsFromSparseMatrix(std::vector<bool> dDefined, std::vector<double> forces, std::vector<double> displacements){
    sparseLinearEquations.values.clear();
    sparseLinearEquations.index.clear();
    sparseLinearEquations.valInds.clear();
    sparseLinearEquations.result.clear();

    // can use symetry to find where things to remove are
    // or just skip straight to edit everything based on index worked out
    std::vector<int> localIndices;
    int ind = 0;
    for (int i = 0; i < dDefined.size(); i++){
        if (dDefined[i]){
            localIndices.push_back(-1);// not in problem also not read ever but stored as -1 so could check
        }
        else{
            localIndices.push_back(ind);
            ind++;
        }
    }

    for (int i = 0; i < sparseGlobalStiffnessMatrix.valInds.size(); i++){
        if (!dDefined[i]){
            std::vector<double> newRow;
            std::vector<int> newInds;
            double val = forces[i];
            for (int j = 0; j < sparseGlobalStiffnessMatrix.valInds[i].size(); j++){
                if (dDefined[sparseGlobalStiffnessMatrix.valInds[i][j]]){
                    val -= sparseGlobalStiffnessMatrix.values[i][j]*displacements[sparseGlobalStiffnessMatrix.valInds[i][j]];
                }
                else{
                    newRow.push_back(sparseGlobalStiffnessMatrix.values[i][j]);
                    newInds.push_back(localIndices[sparseGlobalStiffnessMatrix.valInds[i][j]]);
                    if (i == sparseGlobalStiffnessMatrix.valInds[i][j]){// diagonal
                        sparseLinearEquations.diagonalInd.push_back(newInds.size()-1);
                    }
                }
            }

            sparseLinearEquations.result.push_back(val);
            if (sparseLinearEquations.diagonalInd.size() != sparseLinearEquations.result.size()){
                sparseLinearEquations.diagonalInd.push_back(-1);
            }

            sparseLinearEquations.values.push_back(newRow);
            sparseLinearEquations.valInds.push_back(newInds);
            sparseLinearEquations.index.push_back(i);

        }
    }
    sparseLinearEquations.height = sparseLinearEquations.values.size();
    std::cout << "set up\n";
    std::cout << sparseLinearEquations.height << "\n";
}

void FEStructure::gausianElimination(std::vector<bool> dDefined,std::vector<double> forces,std::vector<double>* displacements){
    makeSystemOfLinearEquations(dDefined, forces, (*displacements));


    int pivotR = 0;
    int pivotC = 0;
    while (pivotC < linearEquations.width && pivotR < linearEquations.height){// forward elimination based on wikipedia pseudo code
        //std::cout << pivotR << " " << pivotC << "\n";
        double absVal = fabs(linearEquations.values[pivotR][pivotC]);// by chosing the row with greatest absval then wont divide by 0 and apparently increases stability with floats
        int pivotRow = pivotR;
        for (int j = pivotR; j < linearEquations.height; j++){// check from non set rows
            if (absVal < fabs(linearEquations.values[j][pivotC])){
                absVal = fabs(linearEquations.values[j][pivotC]);
                pivotRow = j;
            }
        }

        if (absVal <= 0.00000001){// will end up dividing by 0 ish this column is 0 except for already set rows
            pivotC++;
            continue;
        }
        if (pivotC == linearEquations.width-1){// pivoting around last column
            // if this is not 0 as caught above means non unique need more BC
            // eg it may be able to rotate around an axis
            std::cout << linearEquations.index[pivotRow] << "\n";
            throw std::runtime_error("non unique or no solution to displacements");
        }

        if (pivotRow != pivotR){
            for (int j = pivotC; j < linearEquations.width; j++){// swap row so cleaner
                double temp = linearEquations.values[pivotR][j];
                linearEquations.values[pivotR][j] = linearEquations.values[pivotRow][j];
                linearEquations.values[pivotRow][j] = temp;
            }
            int temp = linearEquations.index[pivotR];// update indexes
            linearEquations.index[pivotR] = linearEquations.index[pivotRow];
            linearEquations.index[pivotRow] = temp;
        }

        double sf = 1.0/linearEquations.values[pivotR][pivotC];
        for (int j = 0; j < linearEquations.width; j++){// scale row so first coefficient is one
            linearEquations.values[pivotR][j] *= sf;
        }

        for (int j = pivotR+1; j < linearEquations.height; j++){// eliminate the variable from all rows after this one
            double sf = linearEquations.values[j][pivotC];
            linearEquations.values[j][pivotC] = 0;// for accuarcy reasons
            for (int k = pivotC+1; k < linearEquations.width; k++){
                linearEquations.values[j][k] -= linearEquations.values[pivotR][k]*sf;
            }
        }


        for (int i = 0; i < linearEquations.height; i++){
            for (int j = 0; j < linearEquations.width; j++){
                //std::cout << std::round(echelonStressMatrix[i][j]*1000)/1000.0 << "\t";
            }
            //std::cout << "\n";
        }
        //std::cout << "\n";

        pivotR++;pivotC++;
    }

    std::cout << " done\n";


    for (int i = linearEquations.height-1; i >= 0; i--){
        //std::cout << echelonStressMatrix[i][width-1] << "\n";
        //std::cout << index[i] << "\n";
        (*displacements)[linearEquations.index[i]] = linearEquations.values[i][linearEquations.width-1];
        for (int j = 0; j < i; j++){
            linearEquations.values[j][linearEquations.width-1] -= linearEquations.values[j][i]*linearEquations.values[i][linearEquations.width-1];
        }
    }
}

void FEStructure::jacobiMethod(std::vector<bool> dDefined,std::vector<double> forces,std::vector<double>* displacements, int maxItr){
    //makeSystemOfLinearEquations(dDefined, forces, (*displacements));
    //makeSparseSystemOfLinearEquations(dDefined, forces, (*displacements));
    makeSparseSystemOfLinearEquationsFromSparseMatrix(dDefined, forces, (*displacements));

    /*for (int i = 0; i < sparseLinearEquations.height; i++){
        double sum = -fabs(sparseLinearEquations.values[i][sparseLinearEquations.diagonalInd[i]]);
        for (int j = 0; j < sparseLinearEquations.values[i].size(); j++){
            sum += fabs(sparseLinearEquations.values[i][j]);
            std::cout << sparseLinearEquations.values[i][j] << " " << j << "\n";
        }
        if (fabs(sum) - fabs(sparseLinearEquations.values[i][sparseLinearEquations.diagonalInd[i]]) > 0){
            std::cout << "not diagonally dominant\n";
            std::cout << i << "\n";
            std::cout << fabs(sum) << " " << fabs(sparseLinearEquations.values[i][sparseLinearEquations.diagonalInd[i]]) << "\n";
            std::cout << fabs(sum) - fabs(sparseLinearEquations.values[i][sparseLinearEquations.diagonalInd[i]]) << "\n";
        }
    }*/

    /*for (int test = 0; test < 100; test++){
        double sum = 0;// transpose(x)*A*x if > 0 is positive definite (assuming x != 0)
        std::vector<double> testVals;
        for (int i = 0; i < sparseLinearEquations.height *2; i++){
            double testVal = (rand()/double(RAND_MAX) -0.5)*2;
            testVals.push_back(testVal);
        }
        for (int i = 0; i < sparseLinearEquations.height; i++){
            double val = 0;
            for (int j = 0; j < sparseLinearEquations.values[i].size(); j++){
                val += sparseLinearEquations.values[i][j] * testVals[sparseLinearEquations.index[sparseLinearEquations.valInds[i][j]]];
            }
            sum += val* testVals[sparseLinearEquations.index[i]];
        }
        std::cout << sum << "\n";
        if (sum < 0){
            throw std::runtime_error("not positive definite");
        }
    }
    throw;*/

    // 0.67 tends to infinity after 5000 iterations but 0.66 does not it converges so probably safe below 2/3 for any number of iterations for this problem
    double omega = 0.166;// coefficient which determines how fast it converges too high and not stable too low then too slow working out the optimal is too slow so i just guess
    for (int itr = 0; itr < maxItr; itr++){
        std::cout << itr << "\n";
        double changeSquared = 0;
        std::vector<double> newDisp;
        for (int i = 0; i < sparseLinearEquations.height; i++){
            double x = sparseLinearEquations.result[i];
            for (int j = 0; j < sparseLinearEquations.valInds[i].size(); j++){
                int ind = sparseLinearEquations.valInds[i][j];
                x -= sparseLinearEquations.values[i][j]*(*displacements)[sparseLinearEquations.index[ind]];
            }

            x *= 1.0/sparseLinearEquations.values[i][sparseLinearEquations.diagonalInd[i]];
            if (sparseLinearEquations.values[i][sparseLinearEquations.diagonalInd[i]] == 0){throw;}
            x += (*displacements)[sparseLinearEquations.index[i]];// as I subtracted xi * aii (then divided by aii) need to re-add it doing this here saves having to check if need to continue
            newDisp.push_back(omega*x + (1-omega)*(*displacements)[sparseLinearEquations.index[i]]);
            double thisChange = (*displacements)[sparseLinearEquations.index[i]]-newDisp[i];
            changeSquared += thisChange*thisChange;

            //if (itr < 0){// if not stable this will make it stable but not parallelisable, due to omega = 0.66 unlikely to be needed
                //(*displacements)[index[i]] = newDisp[i];
            //}
        }
        for (int i = 0; i < sparseLinearEquations.height; i++){
            (*displacements)[sparseLinearEquations.index[i]] = newDisp[i];
            //std::cout << newDisp[i] << "\n";
        }
        std::cout << changeSquared << " change\n";
        /*double errorSquared = 0;
        for (int i = 0; i < sparseLinearEquations.height; i++){
            double thisError = -sparseLinearEquations.result[i];
            for (int j = 0; j < sparseLinearEquations.valInds[i].size(); j++){
                int ind = sparseLinearEquations.valInds[i][j];
                thisError += (*displacements)[sparseLinearEquations.index[ind]]*sparseLinearEquations.values[i][j];
            }
            errorSquared += thisError*thisError;
        }
        //std::cout << errorSquared << " error\n";*/
    }
}

void FEStructure::conjugateGradientMethod(std::vector<bool> dDefined,std::vector<double> forces,std::vector<double>* displacements, int maxItr){
    makeSparseSystemOfLinearEquationsFromSparseMatrix(dDefined, forces, (*displacements));

    std::vector<double> r;// = b-Ax
    std::vector<double> p;// = r for first itr
    double rNorm = 0; // r.r the second norm of r squared
    for (int i = 0; i < sparseLinearEquations.height; i++){
        double newVal = sparseLinearEquations.result[i];// b
        for (int j = 0; j < sparseLinearEquations.valInds[i].size(); j++){// -Ax
            newVal -= sparseLinearEquations.values[i][j]*(*displacements)[sparseLinearEquations.index[sparseLinearEquations.valInds[i][j]]];// the indexing system is poorly labled
        }
        r.push_back(newVal);
        p.push_back(newVal);
        rNorm += newVal*newVal;
    }

    if (rNorm == 0){// avoids a division by 0
        return;
    }
    std::cout << rNorm << "init rnorm\n";

    for (int i = 0; i < maxItr; i++){
        std::vector<double> Ap;
        double pAp = 0;
        for (int j = 0; j < sparseLinearEquations.height; j++){
            double val = 0;
            for (int k = 0; k < sparseLinearEquations.valInds[j].size(); k++){
                val += sparseLinearEquations.values[j][k] * p[sparseLinearEquations.valInds[j][k]];
            }
            Ap.push_back(val);
            pAp += p[j]*val;
        }
        double alpha = rNorm/pAp;
        // x += alpha*p
        // r -= alpha*Ap
        double newRNorm = 0;// recalculated from new r value
        if (i%256 == 0){// reset r value periodicaly
            r.clear();
            for (int j = 0; j < sparseLinearEquations.height; j++){
                double newVal = sparseLinearEquations.result[j];// b
                for (int k = 0; k < sparseLinearEquations.valInds[j].size(); k++){// -Ax
                    newVal -= sparseLinearEquations.values[j][k]*(*displacements)[sparseLinearEquations.index[sparseLinearEquations.valInds[j][k]]];// the indexing system is poorly labled
                }
                r.push_back(newVal);
                newRNorm += newVal*newVal;
            }
        }
        else{
            for (int j = 0; j < sparseLinearEquations.index.size(); j++){
                (*displacements)[sparseLinearEquations.index[j]] += alpha*p[j];
                r[j] -= alpha*Ap[j];
                newRNorm += r[j]*r[j];
            }
        }

        //std::cout << pAp << " pAp\n";
        std::cout << rNorm << " Rnorm\n";
        //std::cout << newRNorm << " newRnorm\n";
        //std::cout << alpha << " alpha\n";
        std::cout << i << " i\n";
        //std::cout << newRNorm/rNorm << " rNorm ratio\n";
        //std::cout << (*displacements)[100] << " rand disp\n";

        // once it has converged sufficently there is no point doing anything more
        // if it is below about 10 the human eye should not be able to tell the difference
        // 2.0 is very accurate if it gets this low quit
        if (newRNorm <= 2.0){
            break;
        }
        // if new rNorm is small enough end it has converged

        double beta = newRNorm/rNorm;
        // p = r + beta*p
        for (int j = 0; j < sparseLinearEquations.height; j++){
            p[j] = r[j] + beta*p[j];
        }
        rNorm = newRNorm;
    }
}

void FEStructure::minResMethod(std::vector<bool> dDefined,std::vector<double> forces,std::vector<double>* displacements){

    makeSystemOfLinearEquations( dDefined, forces, (*displacements));

    for (int i = 0; i < linearEquations.height; i++){
        for (int j = 0; j < linearEquations.height; j++){
            if (abs(linearEquations.values[i][j] - linearEquations.values[j][i]) > 0.000001){
                std::cout << "not symetric\n";
                std::cout << linearEquations.values[i][j]-linearEquations.values[j][i] << "\n";
            }
        }
    }

    // r is error
    std::vector<double> r,p0,p1,s0,s1;// based on wikipedia pseudocode
    for (int i = 0; i < linearEquations.height; i++){
        r.push_back(linearEquations.values[i][linearEquations.width-1]);
        for (int j = 0; j < linearEquations.width-1; j++){
            r[i] -= linearEquations.values[i][j]*(*displacements)[linearEquations.index[j]];
        }
        p0.push_back(r[i]);
        p1.push_back(r[i]);
    }
    for (int i = 0; i < linearEquations.height; i++){
        s0.push_back(0);
        for (int j = 0; j < linearEquations.width-1; j++){
            s0[i] += linearEquations.values[i][j]*p0[j];
        }
        s1.push_back(s0[i]);
    }
    int maxItr = 20;

    for (int itr = 0; itr < maxItr; itr++){
        std::cout << itr << "\n";
        std::vector<double> p2,s2;
        for (int i = 0; i < linearEquations.height; i++){
            p2.push_back(p1[i]);
            s2.push_back(s1[i]);
            p1[i] = p0[i];
            s1[i] = s0[i];
        }
        double sumTop = 0;
        double sumBottom = 0;
        for (int i = 0; i < linearEquations.height; i++){
            sumTop += r[i]*s1[i];
            sumBottom += s1[i]*s1[i];
        }
        double a = sumTop/sumBottom;
        std::cout << a << " a\n";
        std::cout << sumTop << " " << sumBottom << "\n";
        double normError = 0;
        for (int i = 0; i < linearEquations.height; i++){
            (*displacements)[linearEquations.index[i]] += a*p1[i];
            r[i] -= a*s1[i];
            normError += r[i]*r[i];
        }
        std::cout << normError << "\n";
        if (normError == 0.01){// could change tolerence
            break;
        }
        sumTop = 0;
        sumBottom = 0;
        for (int i = 0; i < linearEquations.height; i++){
            p0[i] = s1[i];
            s0[i] = 0;
            for (int j = 0; j < linearEquations.width-1; j++){
                s0[i] += linearEquations.values[i][j]*s1[j];
            }
            sumTop += s0[i]*s1[i];
            sumBottom += s1[i]*s1[i];
        }
        double b1 = sumTop/sumBottom;
        std::cout << b1 << " b1\n";
        std::cout << sumTop << " " << sumBottom << "\n";
        for (int i = 0; i < linearEquations.height; i++){
            p0[i] -= b1*p1[i];
            s0[i] -= b1*s1[i];
        }
        if (itr){
            sumTop = 0;
            sumBottom = 0;
            for (int i = 0; i < linearEquations.height; i++){
                sumTop += s0[i]*s2[i];
                sumBottom += s2[i]*s2[i];
            }
            double b2 = sumTop/sumBottom;
            std::cout << b2 << " b2\n";
            std::cout << sumTop << " " << sumBottom << "\n";
            for (int i = 0; i < linearEquations.height; i++){
                p0[i] -= b2*p2[i];
                s0[i] -= b2*s2[i];
            }
        }
    }
}

void FEStructure::makeModel(std::vector<double> displacements, std::vector<double> stresses){
    std::vector<Vertex> vertsV1;
    std::vector<uint16_t> indsV1;
    for (int i = 0; i < numNodes; i++){
        if (surfaceNodes.find(i) != surfaceNodes.end()){
            Vertex newVert;
            float vonMisesStress = sqrt((stresses[i*6]-stresses[i*6+1])*(stresses[i*6]-stresses[i*6+1])
                                      + (stresses[i*6+1]-stresses[i*6+2])*(stresses[i*6+1]-stresses[i*6+2])
                                      + (stresses[i*6+2]-stresses[i*6])*(stresses[i*6+2]-stresses[i*6])
                                      + 6*(stresses[i*6+3]*stresses[i*6+3] + stresses[i*6+4]*stresses[i*6+4] + stresses[i*6+5]*stresses[i*6+5]));
            vonMisesStress = log1p(vonMisesStress*0.3);
            float redMax = static_cast<float>(vonMisesStress>=3);// if above this point set red to always be 1
            glm::vec3 vonMisesColour = glm::vec3(std::max(redMax,-vonMisesStress*vonMisesStress+6*vonMisesStress-8)
                                                ,std::max(0.0f,-vonMisesStress*vonMisesStress+4*vonMisesStress-3)
                                                ,std::max(0.0f,-vonMisesStress*vonMisesStress+2*vonMisesStress));// blue to green to red

            newVert.col = {vonMisesColour.r,vonMisesColour.g,vonMisesColour.b};
            //newVert.col = {fabs(stresses[i*6+0]),fabs(stresses[i*6+1]),fabs(stresses[i*6+2])};// linear stress
            //newVert.col = {fabs(stresses[i*6+3]),fabs(stresses[i*6+4]),fabs(stresses[i*6+5])};// torque
            //newVert.col = {fabs(displacements[i*3]),fabs(displacements[i*3+1]),fabs(displacements[i*3+2])};
            //newVert.col = {1,1,1};
            newVert.pos = {nodes[i*3]/*+displacements[i*3]*/, nodes[i*3+1]/*+displacements[i*3+1]*/, nodes[i*3+2]/*+displacements[i*3+2]*/};
            //newVert.pos = {nodes[i*3]+displacements[i*3], nodes[i*3+1]+displacements[i*3+1], nodes[i*3+2]+displacements[i*3+2]};
            newVert.norm = {0,0,0};
            vertsV1.push_back(newVert);
        }
    }
    for (int i = 0; i < elements.size(); i++){
        std::vector<uint32_t> ind = elements[i]->getVertIndices();
        std::vector<uint32_t> tris;
        switch (ind.size())
        {
        case 4:// tetrahedron
            tris = {ind[0],ind[1],ind[2]
                    ,ind[1],ind[2],ind[3]
                    ,ind[0],ind[2],ind[3]
                    ,ind[0],ind[1],ind[3]};
            for (int i = 0; i < 4; i++){
                if (surfaceNodes.find(tris[i*3]) != surfaceNodes.end() &&
                    surfaceNodes.find(tris[i*3+1]) != surfaceNodes.end() &&
                    surfaceNodes.find(tris[i*3+2]) != surfaceNodes.end()){// if all indexes are surface nodes
                        for (int j = 0; j < 3; j++){
                            indsV1.push_back(surfaceNodes[tris[i*3+j]]);
                        }
                }
            }
            break;
        case 8:// hexahedron
            tris = {ind[0],ind[1],ind[2]
                    ,ind[2],ind[1],ind[3]
                    ,ind[5],ind[4],ind[6]
                    ,ind[5],ind[6],ind[7]

                    ,ind[0],ind[2],ind[4]
                    ,ind[4],ind[2],ind[6]
                    ,ind[3],ind[1],ind[5]
                    ,ind[3],ind[5],ind[7]

                    ,ind[1],ind[0],ind[4]
                    ,ind[1],ind[4],ind[5]
                    ,ind[2],ind[3],ind[6]
                    ,ind[6],ind[3],ind[7]};
            for (int i = 0; i < 12; i++){
                if (surfaceNodes.find(tris[i*3]) != surfaceNodes.end() &&
                    surfaceNodes.find(tris[i*3+1]) != surfaceNodes.end() &&
                    surfaceNodes.find(tris[i*3+2]) != surfaceNodes.end()){// if all indexes are surface nodes
                        for (int j = 0; j < 3; j++){
                            indsV1.push_back(surfaceNodes[tris[i*3+j]]);
                            //std::cout << indsV1[indsV1.size()-1] << " " << tris[i*3+j] << " ";
                        }
                        //std::cout << "\n";
                }
            }
            break;
        default:
            break;
        }
    }
    std::cout << "done make mesh\n";
    /*for (int i = 0; i < numNodes; i++){
        Vertex newVert;
        newVert.col = {1,1,1};
        newVert.pos = {nodes[i*3]+displacements[i*3],nodes[i*3+1]+displacements[i*3+1],nodes[i*3+2]+displacements[i*3+2]};
        newVert.norm = {0,0,0};
        vertsV1.push_back(newVert);
    }
    std::set<uint64_t> surfaceTriangles;
    for(int i = 0; i < elements.size(); i++){
        std::vector<int> ind = elements[i]->getVertIndices();
        int sortInd[4] = {ind[0],ind[1],ind[2],ind[3]};// sort so can be uniquely turned into a uint64 (assuming < 2^16 nodes ( > 2^16 nodes causes other problems))
        // can actually go to 2^21 as triangles stored not tetrahedrons but other problems

        for (int j = 0; j < 3; j++){
            for (int k = 0; k < 3-j; k++){
                if (sortInd[k] < sortInd[k+1]){// sort decreaseing so should be smaller numbers
                    int temp = sortInd[k];
                    sortInd[k] = sortInd[k+1];
                    sortInd[k+1] = temp;
                }
            }
        }
        for (int i = 0; i < 4; i++){
            uint64_t key = 0;// using uint64 as long does not have to be 64 bits on all os
            uint64_t mult = 1;
            for (int j = 0; j < 4; j++){
                if (j == i){continue;}// generate different triangle each time
                key += sortInd[j]*mult;
                mult *= numNodes;
            }
            bool evenParity = (surfaceTriangles.find(key*2) == surfaceTriangles.end());
            bool oddParity = (surfaceTriangles.find(key*2+1) == surfaceTriangles.end());

            if (!evenParity){// if it is in then it is a copy remove original dont add copy
                surfaceTriangles.erase(key*2);
            }
            else if (!oddParity){
                surfaceTriangles.erase(key*2+1);
            }
            else {// if not in add it
                // cross product makes the normal of the triangle (not normalised) then positivity of the dot product says which side other vertex is
                // so i can workout which side the inside is
                bool parity = (0 < glm::dot(vertsV1[sortInd[i]].pos-vertsV1[sortInd[0 + (0 >= i)]].pos// vector from first in triangle to i
                                ,glm::cross(vertsV1[sortInd[1 + (1 >= i)]].pos-vertsV1[sortInd[0 + (0 >= i)]].pos// vector from 2nd to 1st in triangle
                                           ,vertsV1[sortInd[2 + (2 >= i)]].pos-vertsV1[sortInd[0 + (0 >= i)]].pos)));// vector from 3rd to 1st in triangle
                surfaceTriangles.emplace(key*2+parity);
                //std::cout << key << "\n";
            }
        }
    }

    for (auto t = surfaceTriangles.begin(); t != surfaceTriangles.end(); t++){
        uint64_t val = *(t)/2;// should get rid of parity
        bool parity = *(t)%2;
        for (int i = 0; i < 3; i++){
            indsV1.push_back(val%numNodes);
            val /= numNodes;
            //std::cout << indsV1[indsV1.size()-1] << "\n";
        }
        if (parity){
            uint16_t temp = indsV1[indsV1.size()-3];
            indsV1[indsV1.size()-3] = indsV1[indsV1.size()-2];
            indsV1[indsV1.size()-2] = temp;
        }
    }
    std::cout << indsV1.size() << " " << vertsV1.size() << "\n";
    // half of the tris are inside out as winding order not consistent
    // parity is whether or not its wrong

    for (int i = 0; i < indsV1.size()/3; i++){
        glm::vec3 v1 = vertsV1[indsV1[i*3+1]].pos - vertsV1[indsV1[i*3]].pos;
        glm::vec3 v2 = vertsV1[indsV1[i*3+2]].pos - vertsV1[indsV1[i*3]].pos;
        glm::vec3 n = glm::cross(v1,v2);
        for (int j = 0; j < 3; j++){
            vertsV1[indsV1[i*3+j]].norm += n;
        }
    }
    for (int i = 0; i < indsV1.size(); i++){// faster than going through all verts until i remove unused verts even if renormailze multiple times
        vertsV1[indsV1[i]].norm = glm::normalize(vertsV1[indsV1[i]].norm);
    }*/
    std::cout << "nothing extra\n";
    for (int i = 0; i < indsV1.size()/3; i++){
        glm::vec3 v1 = vertsV1[indsV1[i*3+1]].pos - vertsV1[indsV1[i*3]].pos;
        glm::vec3 v2 = vertsV1[indsV1[i*3+2]].pos - vertsV1[indsV1[i*3]].pos;
        glm::vec3 n = glm::cross(v1,v2);
        for (int j = 0; j < 3; j++){
            vertsV1[indsV1[i*3+j]].norm += n;
        }
    }
    for (int i = 0; i < vertsV1.size(); i++){
        vertsV1[i].norm = glm::normalize(vertsV1[i].norm);
    }
    std::cout << "ready\n";
    std::cout << vertsV1.size() << " " << indsV1.size() << "\n";
    p->addModel(vertsV1,indsV1);
}

void FEStructure::solveForStress(std::vector<double> displacements,std::vector<double>* stresses){
    stresses->clear();
    for (int i = 0; i < numNodes*6; i++){
        stresses->push_back(0);
    }
    for (int i = 0; i < elements.size(); i++){// stresses are worked out per element then averaged
        std::vector<uint32_t> ind = elements[i]->getVertIndices();
        std::vector<double> disps = elements[i]->calcNodalSressFromDisplacement(&displacements);
        for (int j = 0; j < ind.size(); j++){
            for (int k = 0; k < 6; k++){
                (*stresses)[ind[j]*6 +k] = disps[j*6 + k];// as this is only used to make approximate pictures it is fine to be wrong as long as good enough
            }
        }
    }
}

void FEStructure::solveStiffnessMatrix(ModelAndForces &mAndF){
    std::cout << "started\n";
    std::vector<double> forces;
    std::vector<double> displacements;
    std::vector<bool> dDefined;// either force is known or displacement is known for a node in a direction so fdefined is not dDefined

    for (int i = 0; i < 3*numNodes; i++){
        forces.push_back(0);
    }
    for (int i = 0; i < 3*numNodes; i++){
        displacements.push_back(0);
        dDefined.push_back(false);
    }

    bool hasExit[3] = {false,false,false};// whether the force has an exit route in this direction
    float cumulativeForce[3] = {0.0,0.0,0.0};//avoid net force which would mean acceleration if not got appropiate exit

    for (int item = 0; item < mAndF.forceFields.size(); item++){
        for (int i = 0; i < numNodes; i++){
            if (mAndF.forceRegions[mAndF.forceFields[item][0]].getSDF(glm::vec3(nodes[i*3+0],nodes[i*3+1],nodes[i*3+2])) <= 0.0){
                int forceType = mAndF.forceFields[item][2];
                switch (forceType)
                {
                case 0:// fix node
                    dDefined[i*3+0] = true;
                    dDefined[i*3+1] = true;
                    dDefined[i*3+2] = true;
                    hasExit[0] = true;
                    hasExit[1] = true;
                    hasExit[2] = true;
                    break;

                case 1:// fix 1d such as if resting on a plane
                    dDefined[i*3+ (mAndF.forceFields[item][3])] = true;
                    hasExit[mAndF.forceFields[item][3]] = true;
                    break;

                case 2:// apply force on node not based on mass or SA
                    forces[i*3+0] += mAndF.forceData[mAndF.forceFields[item][1]][0];
                    forces[i*3+1] += mAndF.forceData[mAndF.forceFields[item][1]][1];
                    forces[i*3+2] += mAndF.forceData[mAndF.forceFields[item][1]][2];
                    cumulativeForce[0] += mAndF.forceData[mAndF.forceFields[item][1]][0];
                    cumulativeForce[1] += mAndF.forceData[mAndF.forceFields[item][1]][1];
                    cumulativeForce[2] += mAndF.forceData[mAndF.forceFields[item][1]][2];
                    break;
                default:
                    break;
                }
            }
        }
    }

    // which node does not matter if only constrain in no exit yet directions
    // therefore can always use node 0

    for (int i = 0; i < 3; i++){
        if (!hasExit[i]){
            dDefined[0*3 +i] = true;
            forces[0*3 +i] += -cumulativeForce[i];
            std::cout << cumulativeForce[i] << "\n";
        }
    }



    //gausianElimination(dDefined,forces,&displacements);
    //jacobiMethod(dDefined,forces,&displacements,500);
    conjugateGradientMethod(dDefined,forces,&displacements,5000);
    //minResMethod(dDefined,forces,&displacements);

    for (int i = 0; i < numNodes; i++){
        //std::cout << displacements[3*i] << " " << displacements[3*i+1] << " " << displacements[3*i+2] << "\n";
    }
    std::vector<double> stresses;
    solveForStress(displacements, &stresses);
    for (int i = 0; i < numNodes; i++){
        //std::cout << stresses[6*i] << " " << stresses[6*i+1] << " " << stresses[6*i+2] << " " << stresses[6*i+3] << " " << stresses[6*i+4] << " " << stresses[6*i+5] << "\n";
    }

    makeModel(displacements,stresses);
}

RigidBody::RigidBody(Model* m, glm::vec4 rot, glm::vec3 loc, glm::mat4x4 rotInertMat, std::vector<Vertex> verts, std::vector<uint16_t> tris){
    for(Vertex v: verts){
        vertices.push_back(v);
    }
    for(uint16_t t: tris){
        triangles.push_back(t);
    }

    model = m;
    pos = loc;
    double axis[3] = {rot[1],rot[2],rot[3]};
    q.set(axis,rot[0]);

    rotInertiaMat = rotInertMat;
    invRotInertia = glm::inverse(rotInertiaMat);
    invMass = 1.0;
    calcTransform();

    p = glm::vec3(0.0,0.0,0.0);
    l = glm::vec3(0.0,0.0,0.0);

    fSum = glm::vec3(0.0,0.0,0.0);
    tSum = glm::vec3(0.0,0.0,0.0);

    glm::vec3 wVec = worldInvRotInertia*l;
    double length = sqrt(wVec[0]*wVec[0]+wVec[1]*wVec[1]+wVec[2]*wVec[2]);
    double rotAxis[3] = {wVec[0]/length,wVec[1]/length,wVec[2]/length};
    if (length == 0){
        rotAxis[0] = 0;rotAxis[1] = 0;rotAxis[2] = 1;
    }
    w.set(rotAxis,length);


    calcAABB();
}

RigidBody::~RigidBody(){

}

void RigidBody::itr(){
    p += fSum;
    l += tSum;

    glm::vec3 wVec = worldInvRotInertia*l;
    double length = sqrt(wVec[0]*wVec[0]+wVec[1]*wVec[1]+wVec[2]*wVec[2]);
    double rotAxis[3] = {wVec[0]/length,wVec[1]/length,wVec[2]/length};
    if (length == 0){
        rotAxis[0] = 0;rotAxis[1] = 0;rotAxis[2] = 1;
    }
    w.set(rotAxis,length);

    fSum[0] = 0.0;fSum[1] = 0.0;fSum[2] = 0.0;
    tSum[0] = 0.0;tSum[1] = 0.0;tSum[2] = 0.0;

    for (int i = 0; i < 3; i++){
        pos[i] += p[i]*invMass;
    }
    glm::vec3 g = glm::vec3(0.0,0.0,-0.00005);
    glm::vec3 c = pos;// apply gravity from center of mass
    calcTransform();// must be done before apply force
    applyForce(g,c);
    w.renormalize();
    q = q.times(w);//w.times(q); // unknown if order correct
    q.renormalize();


    model->setRotation(q);
    float floatPos[3] = {pos[0],pos[1],pos[2]};
    model->moveTo(floatPos);


    //std::cout << l[0] << " " << l[1] << " " << l[2] << " l\n";
    //std::cout << w.q[0] << " " << w.q[1] << " " << w.q[2] << " " << w.q[3] << " vel\n";
    //std::cout << q.q[0] << " " << q.q[1] << " " << q.q[2] << " " << q.q[3] << " rot\n";

    calcTransform();
    calcAABB();
}

void RigidBody::moveBy(glm::vec3 v){
    pos += v;
}

void RigidBody::applyForce(glm::vec3 force,glm::vec3 loc){
    fSum += force;
    tSum += glm::cross(force,loc-pos);//localF[(i+1)%3]*r[(i+2)%3] - localF[(i+2)%3]*r[(i+1%3)];// fxr
}

void RigidBody::calcAABB(){
    double min[3] = {65536,65536,65536};
    double max[3] = {-65536,-65536,-65536};
    for(Vertex v: vertices){
        glm::vec3 vDash = transform*glm::vec4(v.pos,1.0);// where the vertex actually is in world space
        for (int i = 0; i < 3; i++){
            if (vDash[i] < min[i]){
                min[i] = vDash[i];
            }
            if (vDash[i] > max[i]){
                max[i] = vDash[i];
            }
        }
    }
    double size[3] = {max[0]-min[0],max[1]-min[1],max[2]-min[2]};
    for (int i = 0; i < 3; i++){
        aabb.aabbHalfsize[i] = size[i]/2.0;
        aabb.aabbCentre[i] = min[i] + aabb.aabbHalfsize[i];
    }
}

void RigidBody::calcTransform(){
    transform = glm::translate(glm::mat4(1.0f),pos)*q.getMatrix();
    worldRotInertiaMat = glm::mat3x3(glm::transpose(glm::inverse(transform)))*rotInertiaMat*glm::mat3x3(glm::inverse(transform));
    worldInvRotInertia = glm::inverse(worldRotInertiaMat);
}

struct collisionData{
public:
    glm::vec3 loc;
    glm::vec3 n;
    double epsilon;
    double centre;
};

collisionData getCollisionData(std::vector<glm::vec3> obj1v,std::vector<glm::vec3> obj2v,std::vector<uint16_t> obj1i, std::vector<uint16_t> obj2i){// SAT algorithm (idea from stackexchange) I Programed assumes convex polyhedra but could just split concave into convex
    collisionData collision;
    collision.epsilon = -10000.0;
    std::unordered_set<glm::vec3> axes;


    for (int i = 0; i < obj1i.size(); i+=3){// for each triangle
        glm::vec3 v1 = obj1v[obj1i[i+1]]-obj1v[obj1i[i]];// one side of the triangle
        glm::vec3 v2 = obj1v[obj1i[i+2]]-obj1v[obj1i[i]];// another side of the triangle
        glm::vec3 n = glm::cross(v1,v2);// n[i] = v1[(n+1)%3]*v2[(n+2)%3] - v1[(n+2)%3]*v2[(n+1)%3] gives a vector perpendicular to both v1 and v2
        double nLen = sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);// may be faster to not normalize and work it out latter if it does split them
        if (nLen == 0){
            continue;
        }
        //std::cout << nLen << "\n";
        n = n/glm::vec3(nLen);// normalize
        //double d = glm::dot(obj1v[obj1i[i]],n);// where the plane is        not matter as now just projecting
        axes.insert(n);
    }
    for (int i = 0; i < obj2i.size(); i+=3){// for each triangle
        glm::vec3 v1 = obj2v[obj2i[i+1]]-obj2v[obj2i[i]];// one side of the triangle
        glm::vec3 v2 = obj2v[obj2i[i+2]]-obj2v[obj2i[i]];// another side of the triangle
        glm::vec3 n = glm::cross(v1,v2);// n[i] = v1[(n+1)%3]*v2[(n+2)%3] - v1[(n+2)%3]*v2[(n+1)%3] gives a vector perpendicular to both v1 and v2
        double nLen = sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);// may be faster to not normalize and work it out latter if it does split them
        if (nLen == 0){
            continue;
        }
        //std::cout << nLen << "\n";
        n = n/glm::vec3(nLen);// normalize
        //double d = glm::dot(obj1v[obj1i[i]],n);// where the plane is        not matter as now just projecting
        axes.insert(n);
    }
        for (int i = 0; i < obj1i.size(); i++){
        int p2 = (i-(i)%3)+(i+1)%3;// other point to make edge
        glm::vec3 v1 = obj1v[obj1i[(p2)]]-obj1v[obj1i[(i)]];
        for (int j = 0; j < obj2i.size(); j++){
            int p2 = (j-(j)%3)+(j+1)%3;// other point to make edge
            glm::vec3 v2 = obj2v[obj2i[(p2)]]-obj2v[obj2i[(j)]];

            glm::vec3 n = glm::cross(v1,v2);// a plane which both vectors could lie on if moved but not rotated
            double nLen = sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);// may be faster to not normalize and work it out latter if it does split them
            if (nLen == 0){
                continue;
            }
            //std::cout << nLen << "\n";
            n = n/glm::vec3(nLen);// normalize
            //d could be anywhere and it could be assumed to be on one line but it not matter
            axes.insert(n);
        }
    }

    for (glm::vec3 axis: axes){
        double min1 = 65536;
        double min2 = 65536;
        double max1 = -65536;
        double max2 = -65536;
        for (glm::vec3 p: obj1v){// for each of the other's verts
            double pos = glm::dot(axis,p);// take dot product of normal to seperating plane and vert gives where it is projected onto a line
            if (pos < min1){min1 = pos;}
            if (pos > max1){max1 = pos;}
        }
        for (glm::vec3 p: obj2v){// for each of the other's verts
            double pos = glm::dot(axis,p);// take dot product of normal to seperating plane and vert gives where it is projected onto a line
            if (pos < min2){min2 = pos;}
            if (pos > max2){max2 = pos;}
        }

        double overlap;
        double centre;
        if (min1 > max2 || max1 < min2){// no overlap
            if (min1 > max2){
                overlap = max2-min1;// negitive overlap
                centre = (max2+min1)/2.0;
            }
            else{
                overlap = max1-min2;// negitive overlap
                centre = (max1+min2)/2.0;
            }
        }
        else if (min1 <= max2 && min1 >= min2){// 2 overlaps 1
            overlap = max2-min1;
            centre = (max2+min1)/2.0;
            //if (max1 < max2){// 2 contains 1 // messy for collision point so ignore
            //    overlap = max1-min1;
            //    centre = (max1+min1)/2.0;
            //}
        }
        else if (min2 <= max1 && min2 >= min1){// 1 overlaps 2
            overlap = max1-min2;
            centre = (max1+min2)/2.0;
            //if (max2 < max1){// 1 contains 2 // messy so ignore
            //    overlap = max2-min2;
            //    centre = (max2+min2)/2.0;
            //}
        }
        else{
            std::cout << min1 << " " << min2 << " " << max1 << " " << max2 << "\n";
            throw std::runtime_error("logic error in SAT");
        }


        //std::cout << min1 << " " << min2 << " " << max1 << " " << max2 << " mins/maxes\n";
        //std::cout << axis[0] << " " << axis[1] << " " << axis[2] << " " << -overlap << " axis and overlap\n";
        double thisEpsilon = -overlap;// could move normalization to here
        if (thisEpsilon > collision.epsilon){// this is a better division
            collision.epsilon = thisEpsilon;
            collision.n = axis;
            collision.centre = centre;
            if (min2 < min1){// so its always pointing at 2 regular means can later work out whether a point is intersecting or not
                collision.n = -collision.n;
            }
        }
    }

    // now got the collision normal and depth but dont know where on the plane it happened
    double weightSum = 0.0;
    glm::vec3 avePos = glm::vec3(0.0);
    for (int i = 0; i < obj1v.size(); i++){// obj1 should all be less than the plane so > means collision point factor
        double dot = glm::dot(obj1v[i],collision.n);
        if (dot > collision.centre){// is an intersecting point
            avePos += glm::vec3(dot)*obj1v[i];// add this point proportion to how much it is intersecting
            weightSum += dot;// so can renormalize later
        }
    }
    for (int i = 0; i < obj2v.size(); i++){// obj2 should all be more than the plane so < means collision point factor
        double dot = glm::dot(obj2v[i],collision.n);
        if (dot < collision.centre){// is an intersecting point
            avePos += glm::vec3(dot)*obj2v[i];// add this point proportion to how much it is intersecting
            weightSum += dot;// so can renormalize later
        }
    }

    avePos *= glm::vec3(1.0/weightSum);
    collision.loc = avePos;// will be NaNs if no collision as the location where no collision happened is hard to work out
    return collision;
}

void resolveCollision(RigidBody* obj1, RigidBody* obj2){// for use in narrow stage collision detection and resolution

    std::vector<glm::vec3> worldPos1;
    std::vector<uint16_t> inds1;

    std::vector<Vertex>* obj1Vs = obj1->getVertices();
    std::vector<uint16_t>* obj1Ts = obj1->getTriangles();
    glm::mat4x4 transform1 = obj1->getTransformMat();
    for (Vertex v: *(obj1Vs)){
        glm::vec3 vDash = transform1*glm::vec4(v.pos,1.0);
        worldPos1.push_back(vDash);
    }
    for (uint16_t i: *(obj1Ts)){
        inds1.push_back(i);
    }

    std::vector<glm::vec3> worldPos2;
    std::vector<uint16_t> inds2;
    if (obj2 != nullptr){// not collision with floor
        std::vector<Vertex>* obj2Vs = obj2->getVertices();
        std::vector<uint16_t>* obj2Ts = obj2->getTriangles();
        glm::mat4x4 transform2 = obj2->getTransformMat();
        for (Vertex v: *(obj2Vs)){
            glm::vec3 vDash = transform2*glm::vec4(v.pos,1.0);
            worldPos2.push_back(vDash);
        }
        for (uint16_t i: *(obj2Ts)){
            inds2.push_back(i);
        }
    }
    else{

        std::vector<glm::vec3> floorV{
            {64.0,64.0,0.0},
            {-64.0,64.0,0.0},
            {64.0,-64.0,0.0},
            {-64.0,-64.0,0.0}
        };
        std::vector<uint16_t> floorI{
            0,2,1,
            1,3,2
        };
        for (glm::vec3 v: floorV){
            worldPos2.push_back(v);
        }
        for (uint16_t i: floorI){
            inds2.push_back(i);
        }
    }

    double e = 0.4;// 0 means no bounce 1 means perfect bounce  < 0 means ultra stick to surface > 1 means super bounce
    collisionData c = getCollisionData(worldPos1,worldPos2, inds1,inds2);// normal points towards obj2
    //std::cout << c.epsilon << " " << c.n[0] << " " << c.n[1] << " " << c.n[2] << " answer\n";
    //std::cout << c.loc[0] << " " << c.loc[1] << " " << c.loc[2] << " answer\n";

    if (c.epsilon >= 0.0){// no collision to far away
        return;
    }

    if (obj2 == nullptr){// obj2 is the imovable floor

        glm::vec3 r1 = obj1->getPos() - c.loc;
        glm::mat3x3 invI1 = obj1->getWorldInvInertiaTensor();
        double invM1 = obj1->getInvMass();
        glm::vec3 globalRotation = obj1->getAngularVelocity();
        glm::vec3 vRel = (obj1->getVel() + glm::cross(globalRotation,r1));
        glm::vec3 f = c.n*glm::vec3(dot(-glm::vec3(1+e)*vRel,c.n)/(invM1 + dot(cross(invI1*cross(r1,c.n),r1),c.n)));// from wikipedia collision responce


        obj1->applyForce(f,c.loc);
        obj1->moveBy(c.n*glm::vec3(c.epsilon));
        //globalRotation = obj1->getTransformMat()*glm::vec4(obj1->getAngularVelocity(),0.0);
        //std::cout << globalRotation[0] << " " << globalRotation[1] << " " << globalRotation[2] << " global rotation\n";
    }
    else{

        glm::vec3 r1 = obj1->getPos() - c.loc;
        glm::mat3x3 invI1 = obj1->getWorldInvInertiaTensor();
        double invM1 = obj1->getInvMass();

        glm::vec3 r2 = obj2->getPos() - c.loc;
        glm::mat3x3 invI2 = obj2->getWorldInvInertiaTensor();
        double invM2 = obj2->getInvMass();

        glm::vec3 globalRotation1 = obj1->getAngularVelocity();
        glm::vec3 globalRotation2 = obj2->getAngularVelocity();
        glm::vec3 vRel = (obj1->getVel() + glm::cross(globalRotation1,r1))-(obj2->getVel() + glm::cross(globalRotation2,r2));
        glm::vec3 f = c.n*glm::vec3(dot(-glm::vec3(1+e)*vRel,c.n)/
                                    (invM1 + invM2 + dot(cross(invI1*cross(r1,c.n),r1),c.n) + dot(cross(invI1*cross(r1,c.n),r1),c.n)));// from wikipedia collision responce

        obj1->applyForce(f,c.loc);
        obj1->moveBy(c.n*glm::vec3(c.epsilon));
        obj2->applyForce(-f,c.loc);
        obj2->moveBy(-c.n*glm::vec3(c.epsilon));

        //std::cout << f[0] << " " << f[1] << " " << f[2] << " f\n";
        //std::cout << c.loc[0] << " " << c.loc[1] << " " << c.loc[2] << " loc\n";
        //std::cout << c.epsilon << " e\n";
    }
}

bool aabbOverlap(AABB bb1, AABB bb2){
    if ((bb1.aabbCentre[0]-bb1.aabbHalfsize[0] <= bb2.aabbCentre[0]+bb2.aabbHalfsize[0] &&
        bb1.aabbCentre[0]+bb1.aabbHalfsize[0] >= bb2.aabbCentre[0]-bb2.aabbHalfsize[0]) &&
        (bb1.aabbCentre[1]-bb1.aabbHalfsize[1] <= bb2.aabbCentre[1]+bb2.aabbHalfsize[1] &&
        bb1.aabbCentre[1]+bb1.aabbHalfsize[1] >= bb2.aabbCentre[1]-bb2.aabbHalfsize[1]) &&
        (bb1.aabbCentre[2]-bb1.aabbHalfsize[2] <= bb2.aabbCentre[2]+bb2.aabbHalfsize[2] &&
        bb1.aabbCentre[2]+bb1.aabbHalfsize[2] >= bb2.aabbCentre[2]-bb2.aabbHalfsize[2])){
            return true;
    }
    return false;
}

void simulate(std::vector<RigidBody*> rigidBodies){

    for (int i = 0; i < rigidBodies.size(); i++){
        rigidBodies[i]->itr();
    }
    for (int i = 0; i < rigidBodies.size(); i++){
        AABB aabb = rigidBodies[i]->getAABB();
        if (aabb.aabbCentre[2]-aabb.aabbHalfsize[2] < 0.0){// lowest point
            glm::vec3 p = rigidBodies[i]->getMomentum();
            if (p[2] < 0.0){// exists a collision
                resolveCollision(rigidBodies[i],nullptr);
            }
        }
        for (int j = 0; j < i; j++){
            AABB aabb2 = rigidBodies[j]->getAABB();
            if (aabbOverlap(aabb,aabb2)){
                resolveCollision(rigidBodies[i],rigidBodies[j]);
            }
        }
    }
}