# ODE Demo

import sys
import pygame
from pygame.locals import *
import ode
import xode.parser

doc = '''<?xml version="1.0" encoding="iso-8859-1"?>
<xode>
  <world name="world">
    <space>

      <body name="body1">
        <transform>
          <position x="1" y="2" z="0"/>
        </transform>
        
	<mass>
	  <mass_shape density="2500">
	    <sphere radius="0.05"/>
	  </mass_shape>

	</mass>
      </body>

      <body name="body2">
        <transform>
          <position x="2" y="2" z="0"/>
        </transform>
        
	<mass>
	  <mass_shape density="2500">
	    <sphere radius="0.05"/>
	  </mass_shape>
	</mass>

        <joint name="joint2">
          <link1 body="body1"/>
          <ball>
            <anchor x="1" y="2" z="0"/>
          </ball>
        </joint>
      </body>

      <joint name="joint1">
        <link1 body="body1"/>
        <ball>
          <anchor x="0" y="2" z="0"/>
        </ball>
      </joint>

    </space>
  </world>
</xode>'''

def coord(x,y):
    "Convert world coordinates to pixel coordinates."
    return int(320+170*x), int(400-170*y)

def buildObjects():
    world = ode.World()
    world.setGravity((0,-9.81,0))

    body1 = ode.Body(world)
    M = ode.Mass()
    M.setSphere(2500, 0.05)
    body1.setMass(M)
    body1.setPosition((1,2,0))
    
    body2 = ode.Body(world)
    M = ode.Mass()
    M.setSphere(2500, 0.05)
    body2.setMass(M)
    body2.setPosition((2,2,0))
    
    j1 = ode.BallJoint(world)
    j1.attach(body1, ode.environment)
    j1.setAnchor( (0,2,0) )
    #j1 = ode.HingeJoint(world)
    #j1.attach(body1, ode.environment)
    #j1.setAnchor( (0,2,0) )
    #j1.setAxis( (0,0,1) )
    #j1.setParam(ode.ParamVel, 3)
    #j1.setParam(ode.ParamFMax, 22)

    j2 = ode.BallJoint(world)
    j2.attach(body1, body2)
    j2.setAnchor( (1,2,0) )

    return world, body1, body2, j1, j2

def buildObjectsXODE():
    p = xode.parser.Parser()
    root = p.parseString(doc)

    world = root.namedChild('world').getODEObject()
    body1 = root.namedChild('body1').getODEObject()
    body2 = root.namedChild('body2').getODEObject()
    j1 = root.namedChild('joint1').getODEObject()
    j2 = root.namedChild('joint2').getODEObject()

    world.setGravity((0,-9.81,0))
    #body1.setPosition((1,2,0))
    #body2.setPosition((2,2,0))

    #j1 = ode.BallJoint(world)
    #j1.attach(body1, ode.environment)
    #j1.setAnchor( (0,2,0) )

    #j2 = ode.BallJoint(world)
    #j2.attach(body1, body2)
    #j2.setAnchor( (1,2,0) )

    return world, body1, body2, j1, j2

def simulate(world, body1, body2):
    # Initialize pygame
    pygame.init()
    
    # Open a display
    srf = pygame.display.set_mode((640,480))

    clk = pygame.time.Clock()

    # Keep the window open and wait for a key
    fps = 50
    dt = 1.0/fps
    loopFlag = True
    while loopFlag:
        events = pygame.event.get()
        for e in events:
            if e.type==QUIT:
                loopFlag=False
                
        # Clear the screen
        srf.fill((255,255,255))

        x1,y1,z1 = body1.getPosition()
        x2,y2,z2 = body2.getPosition()
        pygame.draw.circle(srf, (55,0,200), coord(x1,y1), 20, 0)
        pygame.draw.line(srf, (55,0,200), coord(0,2), coord(x1,y1), 2)
        pygame.draw.circle(srf, (55,0,200), coord(x2,y2), 20, 0)
        pygame.draw.line(srf, (55,0,200), coord(x1,y1), coord(x2,y2), 2)
        
        pygame.display.flip()

        world.step(dt)
        
        clk.tick(fps)
        #print "fps: %2.1f  dt:%d rawdt:%d"%(clk.get_fps(), clk.get_time(), clk.get_rawtime())


world, body1, body2, j1, j2 = buildObjectsXODE()
simulate(world, body1, body2)
