#### Source code for gravedad.py ####

#!/usr/bin/python
# -*- coding: iso-8859-15 -*-

#***********************************************************************#
#* gravedad.py.  Rafael Treviño. Junio 2006                            *#
#*---------------------------------------------------------------------*#
#* LICENCIA GPL                                                        *#
#*---------------------------------------------------------------------*#
#* Ejemplo "hola mundo" del motor fisico ODE (Open Dynamics Engine)    *#
#* Es un programa para consola, no utiliza representacion en 3D        *#
#* Se crea un mundo virtual y se situa en el una "caja" de una cierta  *#
#* masa y a una cierta altura                                          *#
#* Como salida del programa se obtiene una funcion que indica como     *#
#* varia la altura de la caja con el tiempo, es decir, como cae        *#
#* En el mundo virtual creado NO HAY SUELO, por lo que no hay ninguna  *#
#* colision. El objeto caera infinitamente...                          *#
#*---------------------------------------------------------------------*#
#* Los datos devueltos se pueden visualizar con Octave#Matlab          *#
#* Ejemplo de uso:                                                     *#
#*   $ gravedad.py > func.m                                            *#
#*   $ octave func.m                                                   *#
#***********************************************************************#

import ode
from sys import stdout

#*********************************************#
#* Algunas constantes usadas en el programa  *#
#*********************************************#
##-- Numero de instantes que queremos simular. Este valor se puede
##-- cambiar.
TICKS = 200

##********************************************#
## VARIABLES GLOBALES DEL PROGRAMA           *#
##********************************************#

##-- Identificador para el mundo.
world = None

##-- El objeto que vamos a colocar.
body = None

#***********************************************************************#
#*  CODIGO                                                             *#
#***********************************************************************#

#*********************************************#
#* Crear la "caja"                           *#
#* Se define la "caja" usando la API de ODE  *#
#* y se asocia al "mundo"                    *#
#*********************************************#
def Crear_objeto(): 
	global body, world
	##-- 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 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)

#***********************************************************************#
#* MAIN                                                                *#
#* Primero se crea el mundo (esto es como ser Dios). El mundo es       *#
#* un "contenedor" de objetos                                          *#
#* El mundo no sabe nada de como se dibujan los objetos                *#
#* En este ejemplo el mundo solo tiene una "caja"                      *#
#***********************************************************************#
if __name__ == '__main__': 
	##-- 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)
  
	##-- Crear la "Caja" y ponerla en el "mundo"
	Crear_objeto()

	##-- Salida para Octave:
	##-- La matriz z es la que se ira rellenando con los valores de
	##-- la altura de la caja
	stdout.write('z = [')

	#*************************************#
	#** COMENZAR LA SIMULACION!!!!       *#
	#*************************************#
	##-- Este es el bucle principal. 

	##-- Se haran tantos pasos de simulacion como se indican en la 
	##-- constante TICKS
	for ticks in range(TICKS, 0, -1): 

		##-- Realizar un paso de simulacion.
		##-- Se especifica que resolucion
		##-- en unidades de tiempo se quiere para la simulacion
		world.step(0.01)

		##-- Leer la altura del objeto e imprimirla
		##-- Pos es el vector de posicion, que tiene 3 componentes:
		##-- pos[0] --> x;  pos[1]--> y; pos[2] --> z
		##-- Solo nos interesa la altura a la que esta la caja
		##-- (pos [2])
		pos = body.getPosition()
		stdout.write('%f, '%pos[2])

	##-- Simulacion finalizada:
	##-- Imprimir la ultima posicion e
	##-- Imprimir comandos octave para sacar grafica
	pos = body.getPosition()
	print '%f];'%pos[2]
	print 't = 0:1:%d;'%TICKS
	print 'plot (t, z);'
	print 'grid on;'
	print 'pause;'

	#*************************************#
	#* FIN DE LA SIMULACION              *#
	#*************************************#

	##-- Destruir el mundo con todos sus objetos (apocalipsis?)
	##-- Implícita por Python