#!/usr/bin/python # -*- coding: iso-8859-15 -*- #***********************************************************************# #* gravedad_colision-3D.py. Rafael Treviño. Junio 2006 *# #*---------------------------------------------------------------------*# #* LICENCIA GPL *# #* Programa derivado del ejemplo test_boxstack.cpp que viene con las *# #* fuentes del ODE. Por Russell L. Smith *# #*---------------------------------------------------------------------*# #* Ejemplo "hola mundo" del motor fisico ODE (Open Dynamics Engine) *# #* Es similar al programa gravedad_colision, pero se ha anadido *# #* representacion en 3D, de manera que se puede ver el objeto *# #* cayendo. *# #* Se ha realizado a partir del ejemplo test_boxstack.cpp que viene *# #* en el directorio ode/test de las fuentes del ODE. *# #* Se ha simplificado al maximo y se han anadido comentarios para *# #* que se entienda. *# #*---------------------------------------------------------------------*# #* Es un ejemplo "hola_mundo" que incluye representacion en 3D. *# #* Se coloca una "caja" a una altura, y al comenzar la simulacion *# #* se puede ver como cae. *# #* Los unicos objetos que hay en el mundo son la caja y un plano *# #* que hace de suelo *# #***********************************************************************# import sys import time import ode from sys import stdout #***********************************************************************# #* Sobre la libreria DRAWSTUFF. *# #*---------------------------------------------------------------------*# #* Esta libreria no es parte del motor fisico ODE. Se utiliza para *# #* hacer mas facil el dibujo en 3D *# #* Inicialmente se dibuja una reticula (grid) que tiene 9 puntos. *# #* El punto central (amarillo) es el origen (0,0,0). *# #* El rojo indica la direccion positiva de las Xs *# #* El azul la direccion positiva de las Ys *# #* Estos puntos estan separados una distancia de 1 *# #***********************************************************************# #*********************************************# #* Algunas constantes usadas en el programa *# #*********************************************# ##-- Numero maximo de puntos de contacto. Es para la deteccion de ##-- colisiones. MAX_CONTACTS = 4 ##-- Numero de instantes que queremos simular. Este valor se puede ##-- cambiar TICKS = 200 ##-- Estructura que representa los objetos del universo ##-- Hay dos tipos de elementos: ##-- * Cuerpos (bodys): Tienen la informacion sobre posicion, ##-- orientacion, velocidad lineal y velocidad de rotacion ##-- * Elementos geometricos: Determinal la forma y se usan para ##-- las colisiones ##-- Un objeto esta formado por el cuerpo y su geometria (o la ##-- composicion de varias geometrias. ##-- Se usará una tupla de la forma MyObject = (body, geom) ##********************************************# ## VARIABLES GLOBALES DEL PROGRAMA *# ##********************************************# ##-- Identificador para el mundo world = None ##-- Identificador del espacio (para colisiones) ##-- Para detectar las colisiones hay que crear un espacio con los ##-- elementos que pueden colisionar. space = None ##-- Identificador del grupo de articulaciones de los puntos de contactos ##-- Cuando hay una colision, se crean puntos de contacto entre las ##-- superficies y actuan como articulaciones: los objetos rotaran con ##-- respecto a estos puntos de contacto. contactgroup = None; ##-- El objeto que situamos en el mundo: la caja obj = (None, None); #***********************************************************************# #* CODIGO *# #***********************************************************************# #*********************************************# #* Crear la "caja" *# #* Se define la "caja" usando la API de ODE *# #* y se asocia al "mundo" *# #*********************************************# def Crear_objeto(): global obj, world, space ##-- Identificador para la masa. m = ode.Mass() ##-- Crear el Cuerpo y asociarlo al mundo body = ode.Body(world) ##-- Establecer la posicion inicial. Se pasan las coordenadas x,y,z ##-- En este ejemplo el objeto esta en el origen, a una altura de ##-- 4 unidades body.setPosition((0, 0, 4)) ##-- Establecer la rotacion ##-- Cada objeto puede estar rotado. Se crea una matriz de ##-- rotacion pasandole como parametros el vector que hace de ##-- eje de giro y el angulo que se rota. En este ejemplo se pasa ##-- el eje z y se rota '0' grados con respecto a el. ##-- Probar este mismo ejemplo con los parametros (R, 0, 0, 1, 45) # dRFromAxisAndAngle (R, 0, 0, 1, 0) # obj [0].setRotation (R) ##-- Establecer la masa del cuerpo ##-- Hay que especificar la masa total y las dimensiones ##-- del cubo. Como masa se toma 0.5 y las dimensiones de la caja ##-- son 0.5 x 0.5 x 0.1. Se pueden poner las que se quiera m.setBox(0.5, 0.5, 0.5, 0.1) body.setMass(m) ##-- Crear la geometria: un cubo que se anadira al espacio ##-- para detectar las colisiones. Las dimensiones son ##-- 0.5 x 0.5 x 0.1 geom = ode.GeomBox(space, (0.5, 0.5, 0.1)) ##-- Asociar el cuerpo con la geometria geom.setBody(body); ##-- Creamos el objeto obj = (body, geom) #****************************************************************# #* Funcion de retrollamada invocada por dSpaceCollide cuando *# #* dos objetos del espacio estan a punto de colisionar *# #* El ODE permite que los usuarios avanzados puedan implementar *# #* su propia rutina de colision. Para los usuarios no expertos *# #* que simplemente quieren una colision estandar, esta es la *# #* rutina que SIEMPRE deberan usar. No es solo valida para este *# #* ejemplo, vale para cualquiera. Pero no esta incluida en la *# #* libreria del ODE para que se pueda adaptar a otras *# #* necesidades. *# #****************************************************************# def nearCallback(data, o1, o2): global world, contactgroup ##-- Obtener los cuerpos asociados b1 = o1.getBody() b2 = o2.getBody() ##-- Si ya estan conectados por una articulacion, terminar if b1 and b2 and ode.areConnected(b1, b2): return ##-- Obtener los puntos de contacto, que en realidad son ##-- articulaciones (devuelve una lista) ##-- Para mas informacion consultar la documentacion de ODE ##-- Aqui se puede especificar el tipo de superficies, los ##-- coeficientes de rozamiento (mu), etc... contacts = ode.collide(o1, o2) ##-- Para cada punto de contacto crear una articulacion for c in contacts: ##-- Crear articulacion y meterla en el grupo contactgroup c.setMode(ode.ContactBounce|ode.ContactSoftCFM) c.setMu(ode.Infinity) c.setMu2(0) c.setBounce(0.1) c.setBounceVel(0.1) c.setSoftCFM(0.01) j = ode.ContactJoint(world, contactgroup, c) ##-- Establecer la articulacion entre los dos cuerpos j.attach(b1, b2); #****************************************************************# #* Funcion de retrollamada de comienzo de la simulacion *# #* Se utiliza principalmente para establecer el punto de vista *# #* inicial (posicion de la camara en el universo) *# #****************************************************************# def start(): # print 'Llamada a start ()' ##-- Esta es la posicion de la camara. Se pasan las coordenadas x,y,z xyz = (-6.0, 0.3, 2.0) ##-- Esta es la orientacion de la camara. ##-- Las componentes son Pan, Tilt (inclinacion) y Roll ##-- El valor (0,0,0) indica que la camara esta apuntando en ##-- direccion positiva de las X, paralelamente al suelo. hpr = (0.0, -19.0, 0.0) ##-- Establecer la posicion de la camara drawstuff.dsSetViewport(xyz, hpr) #*********************************************# #* FUNCION DE RETROLLAMADA. *# #* Se invoca cuando se pulsa una tecla *# #*********************************************# def command(cmd): # print 'Llamada a command ()' ##-- En este ejemplo no se usan las teclas print 'Tecla pulsada: %c'%cmd #**************************# #* DIBUJAR una geometria *# #**************************# def drawGeom(g): pos = g.getPosition() R = g.getRotation() sides = g.getLengths() drawstuff.dsDrawBoxD(pos, R, sides) #***********************************************************************# #* Realizar un paso de simulacion. Primero se comprueba colisiones. Si *# #* hay alguna, se crean automaticamente (por medio de la funcion *# #* nearCallback, puntos de contactos que en realidad son articulaciones*# #* Despues se realiza un paso de la simulacion. Los objetos rotaran en *# #* contacto rotaran sobre los nuevos puntos de contacto creados. *# #* Finalmente se eliminan. Si esta en pausa solo de dibuja el universo.*# #***********************************************************************# def simLoop(pause): global world, space, contactgroup # print 'Llamada a simLoop (%d)' % pause if not pause: ##-- Deteccion de colisiones. ##-- Determinar que pares de elementos ##-- geometricos que estan a punto de colisionar. ##-- Se llama a la funcion de retrollamadanear Callback space.collide(None, nearCallback) ##-- Realizar un paso de simulacion world.step(0.01) ## Eliminar todos los puntos de contacto creados. contactgroup.empty() ##-- Dibujar el objeto ##-- Establecer el color drawstuff.dsSetColor(1, 1, 0) ##-- Especificar la textura ##-- Textura DS_WOOD drawstuff.dsSetTexture(1) ##-- Dibujar! drawGeom(obj[1]) ##-- Pequeña pausa para no cargar tanto la CPU ##-- Si la simulacion va lenta se puede eliminar time.sleep(0.01) #***********************************************************************# #* MAIN *# #* Crear el mundo. Es un contenedor de todos los objetos a simular *# #* El mundo no sabe nada de como se dibujan los objetos *# #* En este ejemplo el mundo solo tiene un suelo y una "caja" *# #***********************************************************************# if __name__ == '__main__': try: fd = open('drawstuff.so') except: print 'Antes de ejecutar es necesario hacer "make"' sys.exit(-1) fd.close() import drawstuff ##-- Variable de posicion pos = None ##-- Crear mundo world = ode.World() ##-- Establecer la gravedad (gravedad terrestres: -9.81) world.setGravity((0, 0, -9.81)) ##-- Establecer parametro CFM ##-- Normalmente se deja siempre a este valor world.setCFM(1e-5) ##-- Establecer el modo auto-disabled por defecto ##-- Cualquier objeto que se encuentre en reposo se deshabilitara ##-- y no consumira recursos en la simulacion. Normalmente ##-- siempre se activara world.setAutoDisableFlag(1) ##-- Otros parametros... (consular documentacion) ##-- En principio siempre tendran esos valores world.setContactMaxCorrectingVel(0.1) world.setContactSurfaceLayer(0.001) ##-- Crear un espacio. Los espacios contienen los elementos ##-- geometricos sobre las que se quiere comprobar si hay colision ##-- o no. Se utilizan espacios para que la simulacion sea mas ##-- rapida space = ode.HashSpace() ##-- Crear un grupo de articulaciones ##-- Se utiliza para almacenar los puntos de contacto en una ##-- colision contactgroup = ode.JointGroup() ##-- Crear un plano infinito y meterlo en el espacio ##-- El plano se determina por su ecuacion del tipo: ##-- a*x + b*y + c*z = d, donde (a,b,c) es un vector unitario ##-- normal a su superficie ##-- En este ejemplo se crea el plano con vector (0,0,1), es ##-- decir, el plano: z=0 ##-- Este plano sera para nosotros el "suelo" floor = ode.GeomPlane(space, (0, 0, 1), 0) ##-- Crear la "Caja" y ponerla en el "mundo" Crear_objeto() #*************************************# #** COMENZAR LA SIMULACION!!!! *# #*************************************# ##-- Este es el bucle principal. fn = (2, start, simLoop, command, None, './textures') drawstuff.dsSimulationLoop(len(sys.argv), sys.argv, 400, 300, fn) #*************************************# #* FIN DE LA SIMULACION *# #*************************************# ##-- Destruir el grupo de articulaciones ##-- Destruir el espacio de colisiones ##-- Destruir el mundo con todos sus objetos (apocalipsis?) ##-- Implícitas por Python