From c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 Mon Sep 17 00:00:00 2001 From: sanine Date: Sat, 1 Oct 2022 20:59:36 -0500 Subject: add ode --- libs/ode-0.16.1/bindings/python/demos/tutorial1.py | 49 ++++ libs/ode-0.16.1/bindings/python/demos/tutorial2.py | 135 ++++++++++ libs/ode-0.16.1/bindings/python/demos/tutorial3.py | 284 +++++++++++++++++++++ 3 files changed, 468 insertions(+) create mode 100755 libs/ode-0.16.1/bindings/python/demos/tutorial1.py create mode 100755 libs/ode-0.16.1/bindings/python/demos/tutorial2.py create mode 100644 libs/ode-0.16.1/bindings/python/demos/tutorial3.py (limited to 'libs/ode-0.16.1/bindings/python/demos') diff --git a/libs/ode-0.16.1/bindings/python/demos/tutorial1.py b/libs/ode-0.16.1/bindings/python/demos/tutorial1.py new file mode 100755 index 0000000..c4266d4 --- /dev/null +++ b/libs/ode-0.16.1/bindings/python/demos/tutorial1.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python + +# http://pyode.sourceforge.net/tutorials/tutorial1.html + +# pyODE example 1: Getting started + +# modified by Gideon Klompje (removed literals and using +# 'ode.Mass.setSphereTotal' instead of 'ode.Mass.setSphere') + +import ode + +# Simulation constants +GRAVITY = (0, -9.81, 0) + +SPHERE_RADIUS = 0.05 +SPHERE_MASS = 1.0 +SPHERE_START_POS = (0, 2, 0) +SPHERE_FORCE = (0, 200, 0) # Initial force to apply to the sphere + +TIME_STEP = 0.04 +TIME_STOP = 2.0 # When to stop the simulation + +# Create a world object +world = ode.World() +world.setGravity(GRAVITY) + +# Create a spherical body inside the world +body = ode.Body(world) +mass = ode.Mass() +mass.setSphereTotal(SPHERE_MASS, SPHERE_RADIUS) +body.setMass(mass) + +body.setPosition(SPHERE_START_POS) +body.addForce(SPHERE_FORCE) + +# Do the simulation... +if __name__ == "__main__": + total_time = 0.0 + while total_time < TIME_STOP: + # output the body's position and velocity + x, y, z = body.getPosition() + u, v, w = body.getLinearVel() + print "%1.2fsec: pos=(%6.3f, %6.3f, %6.3f) vel=(%6.3f, %6.3f, %6.3f)" % \ + (total_time, x, y, z, u, v, w) + + # advance the simulation + world.step(TIME_STEP) + total_time += TIME_STEP + diff --git a/libs/ode-0.16.1/bindings/python/demos/tutorial2.py b/libs/ode-0.16.1/bindings/python/demos/tutorial2.py new file mode 100755 index 0000000..1a4a228 --- /dev/null +++ b/libs/ode-0.16.1/bindings/python/demos/tutorial2.py @@ -0,0 +1,135 @@ +#!/usr/bin/env python + +# http://pyode.sourceforge.net/tutorials/tutorial2.html + +# pyODE example 2: Connecting bodies with joints + +# modified by Gideon Klompje (removed literals and using +# 'ode.Mass.setSphereTotal' instead of 'ode.Mass.setSphere') + + +import ode +import pygame + +from pygame.locals import QUIT, KEYDOWN + +# Constants +WINDOW_RESOLUTION = (640, 480) + +DRAW_SCALE = WINDOW_RESOLUTION[0] / 5 +"""Factor to multiply physical coordinates by to obtain screen size in pixels""" + +DRAW_OFFSET = (WINDOW_RESOLUTION[0] / 2, 50) +"""Screen coordinates (in pixels) that map to the physical origin (0, 0, 0)""" + +BACKGROUND_COLOR = (255, 255, 255) + +GRAVITY = (0, -9.81, 0) + +SPHERE1_POSITION = (1, 0, 0) +SPHERE1_MASS = 1 +SPHERE1_RADIUS = 0.15 +SPHERE1_COLOR = (55, 0, 200) + +SPHERE2_POSITION = (2, 0, 0) +SPHERE2_MASS = 1 +SPHERE2_RADIUS = 0.15 +SPHERE2_COLOR = (55, 0, 200) + +JOINT1_ANCHOR = (0, 0, 0) +JOINT1_COLOR = (200, 0, 55) +JOINT1_WIDTH = 2 +"""Width of the line (in pixels) representing the joint""" + +JOINT2_ANCHOR = SPHERE1_POSITION +JOINT2_COLOR = (200, 0, 55) +JOINT2_WIDTH = 2 +"""Width of the line (in pixels) representing the joint""" + +TIME_STEP = 0.04 + +# Utility functions +def coord(x, y, integer=False): + """ + Convert world coordinates to pixel coordinates. Setting 'integer' to + True will return integer coordinates. + """ + xs = (DRAW_OFFSET[0] + DRAW_SCALE*x) + ys = (DRAW_OFFSET[1] - DRAW_SCALE*y) + + if integer: + return int(round(xs)), int(round(ys)) + else: + return xs, ys + +# Initialize pygame +pygame.init() + +# Open a display +screen = pygame.display.set_mode(WINDOW_RESOLUTION) + +# Create a world object +world = ode.World() +world.setGravity(GRAVITY) + +# Create two bodies +body1 = ode.Body(world) +M = ode.Mass() +M.setSphereTotal(SPHERE1_MASS, SPHERE1_RADIUS) +body1.setMass(M) +body1.setPosition(SPHERE1_POSITION) + +body2 = ode.Body(world) +M = ode.Mass() +M.setSphereTotal(SPHERE2_MASS, SPHERE2_RADIUS) +body2.setMass(M) +body2.setPosition(SPHERE2_POSITION) + +# Connect body1 with the static environment +j1 = ode.BallJoint(world) +j1.attach(body1, ode.environment) +j1.setAnchor(JOINT1_ANCHOR) + +# Connect body2 with body1 +j2 = ode.BallJoint(world) +j2.attach(body1, body2) +j2.setAnchor(JOINT2_ANCHOR) + +# Simulation loop... +if __name__ == "__main__": + fps = 1.0 / TIME_STEP + clk = pygame.time.Clock() + + sph1_rad = int(DRAW_SCALE * SPHERE1_RADIUS) + sph2_rad = int(DRAW_SCALE * SPHERE2_RADIUS) + + loopFlag = True + while loopFlag: + for e in pygame.event.get(): + if e.type==QUIT: + loopFlag=False + if e.type==KEYDOWN: + loopFlag=False + + # Clear the screen + screen.fill(BACKGROUND_COLOR) + + # Draw the two bodies and the lines representing the joints + x1, y1, z1 = body1.getPosition() + x2, y2, z2 = body2.getPosition() + xj1, yj1, zj1 = j1.getAnchor() + xj2, yj2, zj2 = j2.getAnchor() + + pygame.draw.line(screen, JOINT1_COLOR, coord(xj1, yj1), coord(x1, y1), JOINT1_WIDTH) + pygame.draw.line(screen, JOINT2_COLOR, coord(xj2, yj2), coord(x2, y2), JOINT2_WIDTH) + pygame.draw.circle(screen, SPHERE1_COLOR, coord(x1, y1, integer=True), sph1_rad, 0) + pygame.draw.circle(screen, SPHERE2_COLOR, coord(x2, y2, integer=True), sph2_rad, 0) + + pygame.display.flip() + + # Next simulation step + world.step(TIME_STEP) + + # Try to keep the specified framerate + clk.tick(fps) + diff --git a/libs/ode-0.16.1/bindings/python/demos/tutorial3.py b/libs/ode-0.16.1/bindings/python/demos/tutorial3.py new file mode 100644 index 0000000..2644d62 --- /dev/null +++ b/libs/ode-0.16.1/bindings/python/demos/tutorial3.py @@ -0,0 +1,284 @@ +#!/usr/bin/env python + +# http://pyode.sourceforge.net/tutorials/tutorial3.html + +# pyODE example 3: Collision detection + +# Originally by Matthias Baas. +# Updated by Pierre Gay to work without pygame or cgkit. + +import sys, os, random, time +from math import * +from OpenGL.GL import * +from OpenGL.GLU import * +from OpenGL.GLUT import * + +import ode + +# geometric utility functions +def scalp (vec, scal): + vec[0] *= scal + vec[1] *= scal + vec[2] *= scal + +def length (vec): + return sqrt (vec[0]**2 + vec[1]**2 + vec[2]**2) + +# prepare_GL +def prepare_GL(): + """Prepare drawing. + """ + + # Viewport + glViewport(0,0,640,480) + + # Initialize + glClearColor(0.8,0.8,0.9,0) + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glEnable(GL_DEPTH_TEST) + glDisable(GL_LIGHTING) + glEnable(GL_LIGHTING) + glEnable(GL_NORMALIZE) + glShadeModel(GL_FLAT) + + # Projection + glMatrixMode(GL_PROJECTION) + glLoadIdentity() + gluPerspective (45,1.3333,0.2,20) + + # Initialize ModelView matrix + glMatrixMode(GL_MODELVIEW) + glLoadIdentity() + + # Light source + glLightfv(GL_LIGHT0,GL_POSITION,[0,0,1,0]) + glLightfv(GL_LIGHT0,GL_DIFFUSE,[1,1,1,1]) + glLightfv(GL_LIGHT0,GL_SPECULAR,[1,1,1,1]) + glEnable(GL_LIGHT0) + + # View transformation + gluLookAt (2.4, 3.6, 4.8, 0.5, 0.5, 0, 0, 1, 0) + +# draw_body +def draw_body(body): + """Draw an ODE body. + """ + + x,y,z = body.getPosition() + R = body.getRotation() + rot = [R[0], R[3], R[6], 0., + R[1], R[4], R[7], 0., + R[2], R[5], R[8], 0., + x, y, z, 1.0] + glPushMatrix() + glMultMatrixd(rot) + if body.shape=="box": + sx,sy,sz = body.boxsize + glScalef(sx, sy, sz) + glutSolidCube(1) + glPopMatrix() + + +# create_box +def create_box(world, space, density, lx, ly, lz): + """Create a box body and its corresponding geom.""" + + # Create body + body = ode.Body(world) + M = ode.Mass() + M.setBox(density, lx, ly, lz) + body.setMass(M) + + # Set parameters for drawing the body + body.shape = "box" + body.boxsize = (lx, ly, lz) + + # Create a box geom for collision detection + geom = ode.GeomBox(space, lengths=body.boxsize) + geom.setBody(body) + + return body, geom + +# drop_object +def drop_object(): + """Drop an object into the scene.""" + + global bodies, geom, counter, objcount + + body, geom = create_box(world, space, 1000, 1.0,0.2,0.2) + body.setPosition( (random.gauss(0,0.1),3.0,random.gauss(0,0.1)) ) + theta = random.uniform(0,2*pi) + ct = cos (theta) + st = sin (theta) + body.setRotation([ct, 0., -st, 0., 1., 0., st, 0., ct]) + bodies.append(body) + geoms.append(geom) + counter=0 + objcount+=1 + +# explosion +def explosion(): + """Simulate an explosion. + + Every object is pushed away from the origin. + The force is dependent on the objects distance from the origin. + """ + global bodies + + for b in bodies: + l=b.getPosition () + d = length (l) + a = max(0, 40000*(1.0-0.2*d*d)) + l = [l[0] / 4, l[1], l[2] /4] + scalp (l, a / length (l)) + b.addForce(l) + +# pull +def pull(): + """Pull the objects back to the origin. + + Every object will be pulled back to the origin. + Every couple of frames there'll be a thrust upwards so that + the objects won't stick to the ground all the time. + """ + global bodies, counter + + for b in bodies: + l=list (b.getPosition ()) + scalp (l, -1000 / length (l)) + b.addForce(l) + if counter%60==0: + b.addForce((0,10000,0)) + +# Collision callback +def near_callback(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: + c.setBounce(0.2) + c.setMu(5000) + j = ode.ContactJoint(world, contactgroup, c) + j.attach(geom1.getBody(), geom2.getBody()) + + + +###################################################################### + +# Initialize Glut +glutInit ([]) + +# Open a window +glutInitDisplayMode (GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE) + +x = 0 +y = 0 +width = 640 +height = 480 +glutInitWindowPosition (x, y); +glutInitWindowSize (width, height); +glutCreateWindow ("testode") + +# Create a world object +world = ode.World() +world.setGravity( (0,-9.81,0) ) +world.setERP(0.8) +world.setCFM(1E-5) + +# Create a space object +space = ode.Space() + +# Create a plane geom which prevent the objects from falling forever +floor = ode.GeomPlane(space, (0,1,0), 0) + +# A list with ODE bodies +bodies = [] + +# The geoms for each of the bodies +geoms = [] + +# A joint group for the contact joints that are generated whenever +# two bodies collide +contactgroup = ode.JointGroup() + +# Some variables used inside the simulation loop +fps = 50 +dt = 1.0/fps +running = True +state = 0 +counter = 0 +objcount = 0 +lasttime = time.time() + + +# keyboard callback +def _keyfunc (c, x, y): + sys.exit (0) + +glutKeyboardFunc (_keyfunc) + +# draw callback +def _drawfunc (): + # Draw the scene + prepare_GL() + for b in bodies: + draw_body(b) + + glutSwapBuffers () + +glutDisplayFunc (_drawfunc) + +# idle callback +def _idlefunc (): + global counter, state, lasttime + + t = dt - (time.time() - lasttime) + if (t > 0): + time.sleep(t) + + counter += 1 + + if state==0: + if counter==20: + drop_object() + if objcount==30: + state=1 + counter=0 + # State 1: Explosion and pulling back the objects + elif state==1: + if counter==100: + explosion() + if counter>300: + pull() + if counter==500: + counter=20 + + glutPostRedisplay () + + # Simulate + n = 4 + + for i in range(n): + # Detect collisions and create contact joints + space.collide((world,contactgroup), near_callback) + + # Simulation step + world.step(dt/n) + + # Remove all contact joints + contactgroup.empty() + + lasttime = time.time() + +glutIdleFunc (_idlefunc) + +glutMainLoop () + -- cgit v1.2.1