MOTEUR PHYSIQUE ODE (PYODE) ET VPYTHON

from visual import *
import ode


class VisualOdeObject(object):
    """
        encapsule un objet de Vpyton
    """
    def __init__(self,name,position):
        self.name = name
        self.position = position
        self.body = None
        
    def draw (self):
        raise NotImplementedError()

    def add_Force (self,value):
           
        self.body.addForce(value)
  
        
class Floor(VisualOdeObject):
    """
        represente le sol
    """

    def __init__(self,space,name, position=(0,0,0), Length = 10, Width = 10):
        VisualOdeObject.__init__(self,name, position)
        
        # Create a plane geom which prevent the objects from falling forever
        self.floor = ode.GeomPlane(space, position, 0)
        self.Length = Length
        self.Width  = Width
      
    def draw (self):
       
        self.View = box(pos     = self.position, 
                        length   = self.Length  ,
                         width   = self.Width ,
                        height   = 0.01 )
                        
class Box(VisualOdeObject):
    """
        represente une boite
    """
    
    def __init__(self,world, space,name, 
                    density,
                    lx, ly, lz,
                    position = (0,2,0), 
                    color= color.red):
                        
        """Create a box body and its corresponding geom."""
        VisualOdeObject.__init__(self,name,position)
        self.lx = lx
        self.ly = ly
        self.lz = lz
        # Create body
        self.body = ode.Body(world)
        M = ode.Mass()
        M.setBox(density, lx, ly, lz)
        self.body.setMass(M)
        self.body.setPosition (self.position)

        # Create a box geom for collision detection
        geom = ode.GeomBox(space, lengths=(lx, ly, lz))
        geom.setBody(self.body)
        
        self.color    = color
        
    def draw (self):
        self.box = box(pos    =  self.position,
                        length =  self.lx,
                        width  = self.ly ,
                        height = self.lz ,
                        color  = self.color)
                         
        self.label1 = label(text= self.name , pos = self.position, opacity =0)
        
        self.vectorVitesseArrow = arrow(pos        = self.position,
                                        axis        = (0,0,0),
                                        shaftwidth  = 0.0001,
                                        color       = color.green)
        
    def redraw (self):
        x,y,z = self.body.getPosition()
        u,v,w = self.body.getLinearVel()

        R = self.body.getRotation()
  
         
        self.box.pos=  x,y,z
        self.box.axis=R[0], R[3], R[6]
        self.box.up = R[1], R[4], R[7]

        self.label1.text= "%s : pos=(%.1f, %.1f, %.1f) "%(self.name,x, y, z)
        self.label1.pos = x, y + 0.5 ,z
        
        self.vectorVitesseArrow.pos  =  x,y,z
        self.vectorVitesseArrow.axis  = [0.1*u,0.1*v,0.1*w]

class Sphere(VisualOdeObject):
    """
       represente une sphere
    """

    def __init__(self,
                    world,
                    space,
                    name,
                    density=1000, 
                    radius=1.0, 
                    position = (0,2,0), 
                    color = color.red):
        
        """Create a sphere body and its corresponding geom."""
        VisualOdeObject.__init__(self,name,position)
        # Create body
        self.body = ode.Body(world)
        self.body.setPosition (self.position)
        
        self.radius   = radius
        self.color    = color
        
        M = ode.Mass()
        M.setSphere(density, radius)
        self.body.setMass(M)

        # Create a box geom for collision detection
        geom = ode.GeomSphere(space, radius)
 
        geom.setBody(self.body)
        
         
    def draw (self):
 
        self.ball1  = sphere(pos =self.position, radius = self.radius, color = self.color)
        
        self.label1 = label(text= self.name , pos = self.position, opacity =0)
        self.vectorVitesseArrow = arrow(pos        = self.position,
                                        axis        = (0,0,0),
                                        shaftwidth  = 0.0001,
                                        color       = color.green)
                                        
    def redraw (self):
         x,y,z= self.body.getPosition()
         u,v,w =self.body.getLinearVel()
          
         self.ball1.pos=  x,y,z
         
         self.label1.text= "%s : pos=(%.1f, %.1f, %.1f) "%(self.name,x, y, z)
         self.label1.pos = x, y + 0.5 ,z # Y is up direction
         self.vectorVitesseArrow.pos  =  x,y,z
         self.vectorVitesseArrow.axis  = vector([0.1*u,0.1*v,0.1*w])

            
class Word_Simulation(object):
    """
        class comment
    """

    def __init__(self, Wordlimit):
        """
        Limit of word : 3-Uplet (X,Z,Y). ex : (10,10,10)
        """
        # create world object (the dynamics world)
        self.world = ode.World()
        self.world.setGravity((0,-9.81,0))
        self.world.setERP(0.8)# error reduction parameter
        self.world.setCFM(1E-5)# constraint force mixing

        self.wordlimit = Wordlimit #Not used Yet

        # Create a space object
        #~ self.space =  ode.Space()
        self.space =  ode.QuadTreeSpace((0, 0, 0), (20, 20, 20), 10) 
        #Objets mobiles
        self.objetsMobilesDict = dict()
        #Objets fixe
        self.objetsFixesList = []

    def addFloor(self, name,position,Length = 10, Width = 10):
        theFloor = Floor(self.space,name, position, Length, Width)
        self.objetsFixesList.append (theFloor )

        
    def addObject (self,name ,TypeOfObject, *parameters,**kw):
        #Creation of an Object that may moved
      
        theObjet = TypeOfObject(world=self.world,space=self.space, name = name, *parameters,**kw)
        self.objetsMobilesDict[name]= theObjet

    def getObjet(self, name):
        return  self.objetsMobilesDict[name]

    def drawObjects (self):
        "Dessine tous les objets fixes une fois"
        for aFixObject in self.objetsFixesList:
            aFixObject.draw()

        for aMobileObject in self.objetsMobilesDict.values():
            aMobileObject.draw()
            
    def launch(self):
        contactgroup = ode.JointGroup()
        loopFlag = True
        total_time = 0.0
        dt = 0.04
        while loopFlag:
            import time
            #~ time.sleep(0.50)

           # Detect collisions and create contact joints
            self.space.collide((self.world,contactgroup), self.process_collision_callback )
            for anObject in self.objetsMobilesDict.values():
                anObject.redraw()
           
            # Next simulation step
            self.world.step(dt)
            total_time+=dt
            # Remove all contact joints
            contactgroup.empty()
            # Try to keep the specified framerate
            rate(25)


    def process_collision_callback(self,args, geom1, geom2):
        """Callback function for the collide() method.

        This function checks if the given geoms do collide and
        creates contact joints if they do.
        """

        # Check if the objects do collide
        contacts = ode.collide(geom1, geom2)

        # Create contact joints
        world,contactgroup = args
        for c in contacts:
            #~ print c
            c.setBounce(0.8) # bouncyness
            c.setMu(800)
            j = ode.ContactJoint(world, contactgroup, c)
            j.attach(geom1.getBody(), geom2.getBody())
            

class ViewWorld(object):
    """
        class comment
    """

    def __init__(self,  worldLimits = vector(10,10,10) ):
        
        #initialize Pyode controler: Wordlimit is 3-uplet, not a  vector wich type belong to theView VPython
        self.theWord_Simulation = Word_Simulation(Wordlimit = (worldLimits.x,worldLimits.y, worldLimits.z))
        
        self.theScene = scene # From Visual
       
        self.worldLimits = worldLimits
        self.initScene()
        
    def initScene(self):
        #~ self.theScene.x           = 100
        #~ self.theScene.y           = 10
        self.theScene.height      = 800
        self.theScene.width       = 800
        self.theScene.forward     = array((0.000001,0,-1))
        self.theScene.scale       = (0.1,0.1,0.1) # X,Z,Y
        self.theScene.title       = "Pyode and Vpython"
        self.theScene.fullscreen  = 1
        self.theScene.background  = [0.94,0.79,0.67]
        self.theScene.center    = (0,1,0) # X,Y,Z : Y is the Up direction
       
        self.theScene.lights      = [vector(-0.1,0.3,0.1)] 
        self.theScene.ambient     = 0.5

    def createScence (self):
        self.theWord_Simulation.addFloor (name = "The Floor 0", 
                position =  (0,0.01,0),
                Length  = self.worldLimits.x,
                Width   =  self.worldLimits.y)
       
     
        self.theWord_Simulation.addObject(name      ="Ref_1",
                                     TypeOfObject= Sphere ,
                                     density     = 100,
                                     radius      = 0.5,
                                     position    = (10,1,10),# X,Y,Z
                                     color       = color.red)
                                     
        self.theWord_Simulation.addObject(name      ="Ref_2",
                                 TypeOfObject= Sphere ,
                                 density     = 100,
                                 radius      = 0.5,
                                 position    = (-10,1,-10),# X,Y,Z
                                 color       = color.red)
                                   
        # Create a Origine
        self.theWord_Simulation.addObject(name      ="Origin",
                                         TypeOfObject= Sphere ,
                                         density     = 1000,
                                         radius      = 0.5,
                                         position    = (0,2,0), #X,Y,Z
                                         color = color.blue)
      
                                         
        self.theWord_Simulation.addObject(name      = "Boite",
                                         TypeOfObject= Box ,
                                         density     = 1000,
                                          lx      = 0.2,
                                          ly     = 0.2,
                                          lz      = 0.2,
                                         position    = (0.1,5,0),
                                         color       = color.green)                                         
        self.addRandomObjects()
        
       
        self.theWord_Simulation.drawObjects()
         
    def addRandomObjects(self):
        from random import Random                            
        sourceRandom = Random()
        WordLength = self.worldLimits.x
        WordWidth  = self.worldLimits.y
        WordHeight = self.worldLimits.z 
        BALL_NUMBER = 100
        for aBallNumber in range(BALL_NUMBER):
            
            colorBall =[  sourceRandom.random(),sourceRandom.random(),sourceRandom.random()]
            posX =  sourceRandom.randrange(0,int(WordLength/2),1) * sourceRandom.choice([1,-1])
            posZ  =  sourceRandom.randrange(0,int(WordWidth/2),1) * sourceRandom.choice([1,-1])
            posY =  sourceRandom.randrange(0,int(WordHeight),1) 
            
            self.theWord_Simulation.addObject(name      ="G_Sphere_%d"%(aBallNumber),
                                             TypeOfObject= Sphere ,
                                             density     = 1000,
                                             radius      = 0.2,
                                             position    = (posX+0.1,posY+0.5,posZ),
                                             color       = colorBall)
            self.theWord_Simulation.addObject(name      = "G_Box_%d"%(aBallNumber),
                                         TypeOfObject= Box ,
                                         density     = 7000,
                                          lx      = 0.2,
                                          ly     = 0.2,
                                          lz      = 0.2,
                                         position    = (posX,posY,posZ),
                                         color       = colorBall)        
  

 
        
    def launchSimulation(self):
        self.theWord_Simulation.launch()
 

aViewWorld = ViewWorld(worldLimits=vector(20,20,20))#X,Y,Z
aViewWorld.createScence()
aViewWorld.launchSimulation()