def setupObjects():
    global bodies, stencilMat
    x = RigidBody(3.0, (3.0,size1[1]/2.0),  shape=size1)
    x.grounded = True
    ypos = [ x.pos[0] + x.shape[0]/2.0 + size2[0]*math.cos(radians(initAngle))/2.0,
             x.pos[1] + x.shape[1]/2.0 + size2[0]*math.sin(radians(initAngle))/2.0]
    y = RigidBody(1.0,  ypos, theta=initAngle, shape=size2)
    bodies = [x, y]
    stencilMat[7, 2] = -y.I
    print stencilMat
def computeForces():
    global bodies, r, gravity, stencilMat
    bodies[0].clearForces()

    #rotate r4 to global frame:
    rg = [r[0]*math.cos(radians(bodies[0].theta)) -
           r[1]*math.sin(radians(bodies[0].theta)),
           r[0]*math.sin(radians(bodies[0].theta)) +
           r[1]*math.cos(radians(bodies[0].theta))]
    
    #print rg

    Amat = np.copy(stencilMat)
    
    Amat[2,3] = rg[1]
    Amat[2,4] = -rg[0]
    Amat[3,2] = rg[1]
    Amat[4,2] = -rg[0]
    
    #print Amat
    b = np.zeros((5,1))

    omega = bodies[0].L/bodies[0].I
    

    b[1,0] = mass*gravity[1]
    b[3,0] = -(omega**2 * rg[0])
    b[4,0] = -(omega**2 * rg[1])


    
    #print b

    soln  = np.linalg.solve(Amat, b)
    #print soln
    #print np.dot(Amat,soln)


    gforce = [comp*bodies[0].mass for comp in gravity] 
    bodies[0].addForce(gforce, (0.0,0.0))
    bodies[0].addForce(tuple(soln[3:]), r)
def computeForces():
    global bodies, r1, r2, r3, r4, gravity, stencilMat
    bodies[0].clearForces()
    bodies[1].clearForces()
    

    #rotate r4 to global frame:
    r4g = [r4[0]*math.cos(radians(bodies[1].theta)) -
           r4[1]*math.sin(radians(bodies[1].theta)),
           r4[0]*math.sin(radians(bodies[1].theta)) +
           r4[1]*math.cos(radians(bodies[1].theta))]
    
    Amat = np.copy(stencilMat)
    
    Amat[2, 3:] =  np.array([[-r1[1], r1[0], -r2[1], r2[0], -r3[1], r3[0]]])
    
    Amat[3, 2] = -r4g[1]
    Amat[4,2] = r4g[0]
    
    Amat[7, 7:] = np.array([[r4g[1], -r4g[0]]])
    print Amat
    b = np.zeros((8,1))
    b[1,0] = -mass1*gravity[1]
    b[6,0] = mass2*gravity[1]
    
    print b

    soln, res, rank, sv = np.linalg.lstsq(Amat, b)

    bodies[0].addForce(gravity, (0.0,0.0))
    bodies[1].addForce(gravity, (0.0,0.0))
    print soln, rank
    #print soln[3:5]
    print Amat*np.matrix(soln)
    bodies[0].addForce(tuple(soln[3:5,0]), r1)
    bodies[0].addForce(tuple(soln[5:7,0]), r2)
    bodies[0].addForce(tuple(soln[7:]), r3)
    
    bodies[1].addForce((-soln[7,0], -soln[8,0]), r4)
def rotate(vec, theta):
    return [vec[0]*math.cos(radians(theta)) -
            vec[1]*math.sin(radians(theta)),
            vec[0]*math.sin(radians(theta)) +
            vec[1]*math.cos(radians(theta))]
size1 = (1.0, 5.0)
mass1 = 3.0

size2 = (2.0, 3.0)
mass2 = 1.0


r1 = [0, -size1[1]/2.0]
r2 = [0, size1[1]/2.0]
r3 = [0, -size2[1]/2.0]

initialPosition1 = ((worldSize[0] + size1[0])/2.0, size1[1]/2.0 + worldSize[1]/2.0)
initAngle = 10.0
initialPosition2 = (initialPosition1[0]  -
                    .5*size2[1]*math.sin(radians(initAngle)),
                    initialPosition1[1] + size1[1]/2.0 +
                    .5*size2[1]*math.cos(radians(initAngle))
                    )

actuationTorque = 0.0


#r = (-size[0]/2.0, -size[1]/2.0)#local vector, global computed each step

gravity = (0, -9.81)

stencilMat = np.matrix(np.zeros((10,10), dtype="float"))
stencilMat[0,6] = -1.0
stencilMat[0,8] = -1.0
stencilMat[1,7] = -1.0
torsor2 = (torsosize[0]/2.0, -torsosize[1]/2.0)
torsor3 = (-torsosize[0]/2.0, torsosize[1]/2.0)
torsor4 = (torsosize[0]/2.0, torsosize[1]/2.0)
torsor5 = (0.0, torsosize[1]/2.0)

headmass = 8.0
headsize = (.15, .25)
headr = (0.0, -headsize[1]/2.0)

initUlAngle = -10.0
initUaAngle = 15.0
initLaAngle = 60.0


ll1pos = (1.5, llsize[1]/2.0)
ul1pos = (ll1pos[0] - ulsize[1]*0.5*math.sin(radians(initUlAngle)),
          ll1pos[1] + llsize[1]/2.0 + ulsize[1]*0.5*math.cos(radians(initUlAngle)))

torsopos = (ul1pos[0] - .5*math.sin(radians(initUlAngle))*ulsize[1] + 
            .5*torsosize[0],
            ul1pos[1] + .5*math.cos(radians(initUlAngle))*ulsize[1] + 
            .5*torsosize[1])

ul2pos = (torsopos[0] + torsosize[0]*.5 - ulsize[1]*.5*
          math.sin(radians(initUlAngle)),
          torsopos[1] - torsosize[1]*.5 - ulsize[1]*.5*
          math.cos(radians(initUlAngle)))

ll2pos = (ul2pos[0] - ulsize[1]*.5*math.sin(radians(initUlAngle)),
          ul2pos[1] - ulsize[1]*.5*math.cos(radians(initUlAngle)) - llsize[1]/2.0)