######################################################################
# Python Open Dynamics Engine Wrapper
# Copyright (C) 2004 PyODE developers (see file AUTHORS)
# All rights reserved.
#
# This library is free software; you can redistribute it and/or
# modify it under the terms of EITHER:
#   (1) The GNU Lesser General Public License as published by the Free
#       Software Foundation; either version 2.1 of the License, or (at
#       your option) any later version. The text of the GNU Lesser
#       General Public License is included with this library in the
#       file LICENSE.
#   (2) The BSD-style license that is included with this library in
#       the file LICENSE-BSD.
#
# This library is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
# LICENSE and LICENSE-BSD for more details.
######################################################################

from ode cimport *


paramLoStop        = 0
paramHiStop        = 1
paramVel           = 2
paramLoVel         = 3
paramHiVel         = 4
paramFMax          = 5
paramFudgeFactor   = 6
paramBounce        = 7
paramCFM           = 8
paramStopERP       = 9
paramStopCFM       = 10
paramSuspensionERP = 11
paramSuspensionCFM = 12
paramERP           = 13

ParamLoStop        = 0
ParamHiStop        = 1
ParamVel           = 2
ParamLoVel         = 3
ParamHiVel         = 4
ParamFMax          = 5
ParamFudgeFactor   = 6
ParamBounce        = 7
ParamCFM           = 8
ParamStopERP       = 9
ParamStopCFM       = 10
ParamSuspensionERP = 11
ParamSuspensionCFM = 12
ParamERP           = 13

ParamLoStop2        = 256 + 0
ParamHiStop2        = 256 + 1
ParamVel2           = 256 + 2
ParamLoVel2         = 256 + 3
ParamHiVel2         = 256 + 4
ParamFMax2          = 256 + 5
ParamFudgeFactor2   = 256 + 6
ParamBounce2        = 256 + 7
ParamCFM2           = 256 + 8
ParamStopERP2       = 256 + 9
ParamStopCFM2       = 256 + 10
ParamSuspensionERP2 = 256 + 11
ParamSuspensionCFM2 = 256 + 12
ParamERP2           = 256 + 13

ParamLoStop3        = 512 + 0
ParamHiStop3        = 512 + 1
ParamVel3           = 512 + 2
ParamLoVel3         = 512 + 3
ParamHiVel3         = 512 + 4
ParamFMax3          = 512 + 5
ParamFudgeFactor3   = 512 + 6
ParamBounce3        = 512 + 7
ParamCFM3           = 512 + 8
ParamStopERP3       = 512 + 9
ParamStopCFM3       = 512 + 10
ParamSuspensionERP3 = 512 + 11
ParamSuspensionCFM3 = 512 + 12
ParamERP3           = 512 + 13

ParamGroup = 256

ContactMu2          = 0x001
ContactAxisDep      = 0x001
ContactFDir1        = 0x002
ContactBounce       = 0x004
ContactSoftERP      = 0x008
ContactSoftCFM      = 0x010
ContactMotion1      = 0x020
ContactMotion2      = 0x040
ContactMotionN      = 0x080
ContactSlip1        = 0x100
ContactSlip2        = 0x200
ContactRolling      = 0x400

ContactApprox0      = 0x0000
ContactApprox1_1    = 0x1000
ContactApprox1_2    = 0x2000
ContactApprox1_N    = 0x4000
ContactApprox1      = 0x7000

AMotorUser = dAMotorUser
AMotorEuler = dAMotorEuler

Infinity = dInfinity


import weakref
_geom_c2py_lut = weakref.WeakValueDictionary()


cdef class Mass:
    """Mass parameters of a rigid body.

    This class stores mass parameters of a rigid body which can be
    accessed through the following attributes:

     - mass: The total mass of the body (float)
     - c:    The center of gravity position in body frame (3-tuple of floats)
     - I:    The 3x3 inertia tensor in body frame (3-tuple of 3-tuples)

    This class wraps the dMass structure from the C API.

    @ivar mass: The total mass of the body
    @ivar c: The center of gravity position in body frame (cx, cy, cz)
    @ivar I: The 3x3 inertia tensor in body frame ((I11, I12, I13), (I12, I22, I23), (I13, I23, I33))
    @type mass: float
    @type c: 3-tuple of floats
    @type I: 3-tuple of 3-tuples of floats
    """
    cdef dMass _mass

    def __cinit__(self):
        dMassSetZero(&self._mass)

    def setZero(self):
        """setZero()

        Set all the mass parameters to zero."""
        dMassSetZero(&self._mass)

    def setParameters(self, mass, cgx, cgy, cgz, I11, I22, I33, I12, I13, I23):
        """setParameters(mass, cgx, cgy, cgz, I11, I22, I33, I12, I13, I23)

        Set the mass parameters to the given values.

        @param mass: Total mass of the body.
        @param cgx: Center of gravity position in the body frame (x component).
        @param cgy: Center of gravity position in the body frame (y component).
        @param cgz: Center of gravity position in the body frame (z component).
        @param I11: Inertia tensor
        @param I22: Inertia tensor
        @param I33: Inertia tensor
        @param I12: Inertia tensor
        @param I13: Inertia tensor
        @param I23: Inertia tensor
        @type mass: float
        @type cgx: float
        @type cgy: float
        @type cgz: float
        @type I11: float
        @type I22: float
        @type I33: float
        @type I12: float
        @type I13: float
        @type I23: float
        """
        dMassSetParameters(&self._mass, mass, cgx, cgy, cgz,
                           I11, I22, I33, I12, I13, I23)

    def setSphere(self, density, radius):
        """setSphere(density, radius)
        
        Set the mass parameters to represent a sphere of the given radius
        and density, with the center of mass at (0,0,0) relative to the body.

        @param density: The density of the sphere
        @param radius: The radius of the sphere
        @type density: float
        @type radius: float
        """
        dMassSetSphere(&self._mass, density, radius)

    def setSphereTotal(self, total_mass, radius):
        """setSphereTotal(total_mass, radius)
        
        Set the mass parameters to represent a sphere of the given radius
        and mass, with the center of mass at (0,0,0) relative to the body.

        @param total_mass: The total mass of the sphere
        @param radius: The radius of the sphere
        @type total_mass: float
        @type radius: float
        """
        dMassSetSphereTotal(&self._mass, total_mass, radius)

    def setCapsule(self, density, direction, radius, length):
        """setCapsule(density, direction, radius, length)

        Set the mass parameters to represent a capsule of the given parameters
        and density, with the center of mass at (0,0,0) relative to the body.
        The radius of the cylinder (and the spherical cap) is radius. The length
        of the cylinder (not counting the spherical cap) is length. The
        cylinder's long axis is oriented along the body's x, y or z axis
        according to the value of direction (1=x, 2=y, 3=z). The first function
        accepts the density of the object, the second accepts its total mass.

        @param density: The density of the capsule
        @param direction: The direction of the capsule's cylinder (1=x axis, 2=y axis, 3=z axis)
        @param radius: The radius of the capsule's cylinder
        @param length: The length of the capsule's cylinder (without the caps)
        @type density: float
        @type direction: int
        @type radius: float
        @type length: float
        """
        dMassSetCapsule(&self._mass, density, direction, radius, length)

    def setCapsuleTotal(self, total_mass, direction, radius, length):
        """setCapsuleTotal(total_mass, direction, radius, length)

        Set the mass parameters to represent a capsule of the given parameters
        and mass, with the center of mass at (0,0,0) relative to the body. The
        radius of the cylinder (and the spherical cap) is radius. The length of
        the cylinder (not counting the spherical cap) is length. The cylinder's
        long axis is oriented along the body's x, y or z axis according to the
        value of direction (1=x, 2=y, 3=z). The first function accepts the
        density of the object, the second accepts its total mass.

        @param total_mass: The total mass of the capsule
        @param direction: The direction of the capsule's cylinder (1=x axis, 2=y axis, 3=z axis)
        @param radius: The radius of the capsule's cylinder
        @param length: The length of the capsule's cylinder (without the caps)
        @type total_mass: float
        @type direction: int
        @type radius: float
        @type length: float
        """
        dMassSetCapsuleTotal(&self._mass, total_mass, direction,
                             radius, length)

    def setCylinder(self, density, direction, r, h):
        """setCylinder(density, direction, r, h)
        
        Set the mass parameters to represent a flat-ended cylinder of
        the given parameters and density, with the center of mass at
        (0,0,0) relative to the body. The radius of the cylinder is r.
        The length of the cylinder is h. The cylinder's long axis is
        oriented along the body's x, y or z axis according to the value
        of direction (1=x, 2=y, 3=z).

        @param density: The density of the cylinder
        @param direction: The direction of the cylinder (1=x axis, 2=y axis, 3=z axis)
        @param r: The radius of the cylinder
        @param h: The length of the cylinder
        @type density: float
        @type direction: int
        @type r: float
        @type h: float
        """
        dMassSetCylinder(&self._mass, density, direction, r, h)

    def setCylinderTotal(self, total_mass, direction, r, h):
        """setCylinderTotal(total_mass, direction, r, h)
        
        Set the mass parameters to represent a flat-ended cylinder of
        the given parameters and mass, with the center of mass at
        (0,0,0) relative to the body. The radius of the cylinder is r.
        The length of the cylinder is h. The cylinder's long axis is
        oriented along the body's x, y or z axis according to the value
        of direction (1=x, 2=y, 3=z).

        @param total_mass: The total mass of the cylinder
        @param direction: The direction of the cylinder (1=x axis, 2=y axis, 3=z axis)
        @param r: The radius of the cylinder
        @param h: The length of the cylinder
        @type total_mass: float
        @type direction: int
        @type r: float
        @type h: float
        """
        dMassSetCylinderTotal(&self._mass, total_mass, direction, r, h)

    def setBox(self, density, lx, ly, lz):
        """setBox(density, lx, ly, lz)

        Set the mass parameters to represent a box of the given
        dimensions and density, with the center of mass at (0,0,0)
        relative to the body. The side lengths of the box along the x,
        y and z axes are lx, ly and lz.

        @param density: The density of the box
        @param lx: The length along the x axis
        @param ly: The length along the y axis
        @param lz: The length along the z axis
        @type density: float
        @type lx: float
        @type ly: float
        @type lz: float
        """
        dMassSetBox(&self._mass, density, lx, ly, lz)

    def setBoxTotal(self, total_mass, lx, ly, lz):
        """setBoxTotal(total_mass, lx, ly, lz)

        Set the mass parameters to represent a box of the given
        dimensions and mass, with the center of mass at (0,0,0)
        relative to the body. The side lengths of the box along the x,
        y and z axes are lx, ly and lz.

        @param total_mass: The total mass of the box
        @param lx: The length along the x axis
        @param ly: The length along the y axis
        @param lz: The length along the z axis
        @type total_mass: float
        @type lx: float
        @type ly: float
        @type lz: float
        """
        dMassSetBoxTotal(&self._mass, total_mass, lx, ly, lz)

    def adjust(self, newmass):
        """adjust(newmass)

        Adjust the total mass. Given mass parameters for some object,
        adjust them so the total mass is now newmass. This is useful
        when using the setXyz() methods to set the mass parameters for
        certain objects - they take the object density, not the total
        mass.

        @param newmass: The new total mass
        @type newmass: float
        """
        dMassAdjust(&self._mass, newmass)

    def translate(self, t):
        """translate(t)

        Adjust mass parameters. Given mass parameters for some object,
        adjust them to represent the object displaced by (x,y,z)
        relative to the body frame.

        @param t: Translation vector (x, y, z)
        @type t: 3-tuple of floats
        """
        dMassTranslate(&self._mass, t[0], t[1], t[2])

#    def rotate(self, R):
#        """
#        Given mass parameters for some object, adjust them to
#        represent the object rotated by R relative to the body frame.
#        """
#        pass

    def add(self, Mass b):
        """add(b)

        Add the mass b to the mass object. Masses can also be added using
        the + operator.

        @param b: The mass to add to this mass
        @type b: Mass
        """
        dMassAdd(&self._mass, &b._mass)

    def __getattr__(self, name):
        if name == "mass":
            return self._mass.mass
        elif name == "c":
            return self._mass.c[0], self._mass.c[1], self._mass.c[2]
        elif name == "I":
            return ((self._mass.I[0], self._mass.I[1], self._mass.I[2]),
                    (self._mass.I[4], self._mass.I[5], self._mass.I[6]),
                    (self._mass.I[8], self._mass.I[9], self._mass.I[10]))
        else:
            raise AttributeError("Mass object has no attribute '%s'" % name)

    def __setattr__(self, name, value):
        if name == "mass":
            self.adjust(value)
        elif name == "c":
            raise AttributeError("Use the setParameter() method to change c")
        elif name == "I":
            raise AttributeError("Use the setParameter() method to change I")
        else:
            raise AttributeError("Mass object has no attribute '%s" % name)

    def __add__(self, Mass b):
        self.add(b)
        return self

    def __str__(self):
        m = str(self._mass.mass)
        sc0 = str(self._mass.c[0])
        sc1 = str(self._mass.c[1])
        sc2 = str(self._mass.c[2])
        I11 = str(self._mass.I[0])
        I22 = str(self._mass.I[5])
        I33 = str(self._mass.I[10])
        I12 = str(self._mass.I[1])
        I13 = str(self._mass.I[2])
        I23 = str(self._mass.I[6])
        return ("Mass=%s\n"
                "Cg=(%s, %s, %s)\n"
                "I11=%s I22=%s I33=%s\n"
                "I12=%s I13=%s I23=%s" %
                (m, sc0, sc1, sc2, I11, I22, I33, I12, I13, I23))
#        return ("Mass=%s / "
#                "Cg=(%s, %s, %s) / "
#                "I11=%s I22=%s I33=%s "
#                "I12=%s I13=%s I23=%s" %
#                (m, sc0, sc1, sc2, I11, I22, I33, I12, I13, I23))


cdef class Contact:
    """This class represents a contact between two bodies in one point.

    A Contact object stores all the input parameters for a ContactJoint.
    This class wraps the ODE dContact structure which has 3 components::

     struct dContact {
       dSurfaceParameters surface;
       dContactGeom geom;
       dVector3 fdir1;
     };

    This wrapper class provides methods to get and set the items of those
    structures.
    """
    
    cdef dContact _contact

    def __cinit__(self):
        self._contact.surface.mode = ContactBounce
        self._contact.surface.mu = dInfinity

        self._contact.surface.bounce = 0.1

    # getMode
    def getMode(self):
        """getMode() -> flags

        Return the contact flags.
        """
        return self._contact.surface.mode

    # setMode
    def setMode(self, flags):
        """setMode(flags)

        Set the contact flags. The argument m is a combination of the
        ContactXyz flags (ContactMu2, ContactBounce, ...).
 
        @param flags: Contact flags
        @type flags: int
        """
        self._contact.surface.mode = flags

    # getMu
    def getMu(self):
        """getMu() -> float

        Return the Coulomb friction coefficient.
        """
        return self._contact.surface.mu

    # setMu
    def setMu(self, mu):
        """setMu(mu)

        Set the Coulomb friction coefficient.

        @param mu: Coulomb friction coefficient (0..Infinity)
        @type mu: float
        """
        self._contact.surface.mu = mu

    # getMu2
    def getMu2(self):
        """getMu2() -> float

        Return the optional Coulomb friction coefficient for direction 2.
        """
        return self._contact.surface.mu2

    # setMu2
    def setMu2(self, mu):
        """setMu2(mu)

        Set the optional Coulomb friction coefficient for direction 2.

        @param mu: Coulomb friction coefficient (0..Infinity)
        @type mu: float
        """
        self._contact.surface.mu2 = mu

    # getBounce
    def getBounce(self):
        """getBounce() -> float

        Return the restitution parameter.
        """
        return self._contact.surface.bounce

    # setBounce
    def setBounce(self, b):
        """setBounce(b)

        @param b: Restitution parameter (0..1)
        @type b: float
        """
        self._contact.surface.bounce = b

    # getBounceVel
    def getBounceVel(self):
        """getBounceVel() -> float

        Return the minimum incoming velocity necessary for bounce.
        """
        return self._contact.surface.bounce_vel

    # setBounceVel
    def setBounceVel(self, bv):
        """setBounceVel(bv)

        Set the minimum incoming velocity necessary for bounce. Incoming
        velocities below this will effectively have a bounce parameter of 0.

        @param bv: Velocity
        @type bv: float
        """
        self._contact.surface.bounce_vel = bv

    # getSoftERP
    def getSoftERP(self):
        """getSoftERP() -> float

        Return the contact normal "softness" parameter.
        """
        return self._contact.surface.soft_erp

    # setSoftERP
    def setSoftERP(self, erp):
        """setSoftERP(erp)

        Set the contact normal "softness" parameter.

        @param erp: Softness parameter
        @type erp: float
        """
        self._contact.surface.soft_erp = erp

    # getSoftCFM
    def getSoftCFM(self):
        """getSoftCFM() -> float

        Return the contact normal "softness" parameter.
        """
        return self._contact.surface.soft_cfm

    # setSoftCFM
    def setSoftCFM(self, cfm):
        """setSoftCFM(cfm)

        Set the contact normal "softness" parameter.

        @param cfm: Softness parameter
        @type cfm: float
        """
        self._contact.surface.soft_cfm = cfm

    # getMotion1
    def getMotion1(self):
        """getMotion1() -> float

        Get the surface velocity in friction direction 1.
        """
        return self._contact.surface.motion1

    # setMotion1
    def setMotion1(self, m):
        """setMotion1(m)

        Set the surface velocity in friction direction 1.

        @param m: Surface velocity
        @type m: float
        """
        self._contact.surface.motion1 = m

    # getMotion2
    def getMotion2(self):
        """getMotion2() -> float

        Get the surface velocity in friction direction 2.
        """
        return self._contact.surface.motion2

    # setMotion2
    def setMotion2(self, m):
        """setMotion2(m)

        Set the surface velocity in friction direction 2.

        @param m: Surface velocity
        @type m: float
        """
        self._contact.surface.motion2 = m

    # getSlip1
    def getSlip1(self):
        """getSlip1() -> float

        Get the coefficient of force-dependent-slip (FDS) for friction
        direction 1.
        """
        return self._contact.surface.slip1

    # setSlip1
    def setSlip1(self, s):
        """setSlip1(s)

        Set the coefficient of force-dependent-slip (FDS) for friction
        direction 1.

        @param s: FDS coefficient
        @type s: float
        """
        self._contact.surface.slip1 = s

    # getSlip2
    def getSlip2(self):
        """getSlip2() -> float

        Get the coefficient of force-dependent-slip (FDS) for friction
        direction 2.
        """
        return self._contact.surface.slip2

    # setSlip2
    def setSlip2(self, s):
        """setSlip2(s)

        Set the coefficient of force-dependent-slip (FDS) for friction
        direction 1.

        @param s: FDS coefficient
        @type s: float
        """
        self._contact.surface.slip2 = s

    # getFDir1
    def getFDir1(self):
        """getFDir1() -> (x, y, z)

        Get the "first friction direction" vector that defines a direction
        along which frictional force is applied.
        """
        return (self._contact.fdir1[0],
                self._contact.fdir1[1],
                self._contact.fdir1[2])

    # setFDir1
    def setFDir1(self, fdir):
        """setFDir1(fdir)

        Set the "first friction direction" vector that defines a direction
        along which frictional force is applied. It must be of unit length
        and perpendicular to the contact normal (so it is typically
        tangential to the contact surface).

        @param fdir: Friction direction
        @type fdir: 3-sequence of floats
        """
        self._contact.fdir1[0] = fdir[0]
        self._contact.fdir1[1] = fdir[1]
        self._contact.fdir1[2] = fdir[2]

    # getContactGeomParams
    def getContactGeomParams(self):
        """getContactGeomParams() -> (pos, normal, depth, geom1, geom2)

        Get the ContactGeom structure of the contact.

        The return value is a tuple (pos, normal, depth, geom1, geom2)
        where pos and normal are 3-tuples of floats and depth is a single
        float. geom1 and geom2 are the Geom objects of the geoms in contact.
        """
        cdef size_t id1, id2

        pos = (self._contact.geom.pos[0],
               self._contact.geom.pos[1],
               self._contact.geom.pos[2])
        normal = (self._contact.geom.normal[0],
                  self._contact.geom.normal[1],
                  self._contact.geom.normal[2])
        depth = self._contact.geom.depth

        id1 = <size_t>self._contact.geom.g1
        id2 = <size_t>self._contact.geom.g2
        g1 = _geom_c2py_lut[id1]
        g2 = _geom_c2py_lut[id2]
        return pos, normal, depth, g1, g2

    # setContactGeomParams
    def setContactGeomParams(self, pos, normal, depth, g1=None, g2=None):
        """setContactGeomParams(pos, normal, depth, geom1=None, geom2=None)
        
        Set the ContactGeom structure of the contact.

        @param pos:  Contact position, in global coordinates
        @type pos: 3-sequence of floats
        @param normal: Unit length normal vector
        @type normal: 3-sequence of floats
        @param depth: Depth to which the two bodies inter-penetrate
        @type depth: float
        @param geom1: Geometry object 1 that collided
        @type geom1: Geom
        @param geom2: Geometry object 2 that collided
        @type geom2: Geom
        """

        cdef size_t id

        self._contact.geom.pos[0] = pos[0]
        self._contact.geom.pos[1] = pos[1]
        self._contact.geom.pos[2] = pos[2]
        self._contact.geom.normal[0] = normal[0]
        self._contact.geom.normal[1] = normal[1]
        self._contact.geom.normal[2] = normal[2]
        self._contact.geom.depth = depth
        if g1 != None:
            id = g1._id()
            self._contact.geom.g1 = <dGeomID>id
        else:
            self._contact.geom.g1 = <dGeomID>0
            
        if g2 != None:
            id = g2._id()
            self._contact.geom.g2 = <dGeomID>id
        else:
            self._contact.geom.g2 = <dGeomID>0


# World
cdef class World:
    """Dynamics world.
    
    The world object is a container for rigid bodies and joints.
    
    
    Constructor::
    
      World()
    """

    cdef dWorldID wid

    def __cinit__(self):
        self.wid = dWorldCreate()

    def __dealloc__(self):
        if self.wid != NULL:
            dWorldDestroy(self.wid)

    # setGravity
    def setGravity(self, gravity):
        """setGravity(gravity)

        Set the world's global gravity vector.

        @param gravity: Gravity vector
        @type gravity: 3-sequence of floats
        """
        dWorldSetGravity(self.wid, gravity[0], gravity[1], gravity[2])

    # getGravity
    def getGravity(self):
        """getGravity() -> 3-tuple

        Return the world's global gravity vector as a 3-tuple of floats.
        """
        cdef dVector3 g
        dWorldGetGravity(self.wid, g)
        return g[0], g[1], g[2]

    # setERP
    def setERP(self, erp):
        """setERP(erp)

        Set the global ERP value, that controls how much error
        correction is performed in each time step. Typical values are
        in the range 0.1-0.8. The default is 0.2.

        @param erp: Global ERP value
        @type erp: float
        """
        dWorldSetERP(self.wid, erp)

    # getERP
    def getERP(self):
        """getERP() -> float

        Get the global ERP value, that controls how much error
        correction is performed in each time step. Typical values are
        in the range 0.1-0.8. The default is 0.2.
        """
        return dWorldGetERP(self.wid)

    # setCFM
    def setCFM(self, cfm):
        """setCFM(cfm)

        Set the global CFM (constraint force mixing) value. Typical
        values are in the range 10E-9 - 1. The default is 10E-5 if
        single precision is being used, or 10E-10 if double precision
        is being used.

        @param cfm: Constraint force mixing value
        @type cfm: float
        """
        dWorldSetCFM(self.wid, cfm)

    # getCFM
    def getCFM(self):
        """getCFM() -> float

        Get the global CFM (constraint force mixing) value. Typical
        values are in the range 10E-9 - 1. The default is 10E-5 if
        single precision is being used, or 10E-10 if double precision
        is being used.
        """
        return dWorldGetCFM(self.wid)

    # step
    def step(self, stepsize):
        """step(stepsize)

        Step the world. This uses a "big matrix" method that takes
        time on the order of O(m3) and memory on the order of O(m2), where m
        is the total number of constraint rows.

        For large systems this will use a lot of memory and can be
        very slow, but this is currently the most accurate method.

        @param stepsize: Time step
        @type stepsize: float
        """

        dWorldStep(self.wid, stepsize)

    # quickStep
    def quickStep(self, stepsize):
        """quickStep(stepsize)
        
        Step the world. This uses an iterative method that takes time
        on the order of O(m*N) and memory on the order of O(m), where m is
        the total number of constraint rows and N is the number of
        iterations.

        For large systems this is a lot faster than dWorldStep, but it
        is less accurate.

        @param stepsize: Time step
        @type stepsize: float
        """
        dWorldQuickStep(self.wid, stepsize)

    # setQuickStepNumIterations
    def setQuickStepNumIterations(self, num):
        """setQuickStepNumIterations(num)
        
        Set the number of iterations that the QuickStep method
        performs per step. More iterations will give a more accurate
        solution, but will take longer to compute. The default is 20
        iterations.

        @param num: Number of iterations
        @type num: int
        """
        
        dWorldSetQuickStepNumIterations(self.wid, num)

    # getQuickStepNumIterations
    def getQuickStepNumIterations(self):
        """getQuickStepNumIterations() -> int
        
        Get the number of iterations that the QuickStep method
        performs per step. More iterations will give a more accurate
        solution, but will take longer to compute. The default is 20
        iterations.
        """
        return dWorldGetQuickStepNumIterations(self.wid)

    # setQuickStepNumIterations
    def setContactMaxCorrectingVel(self, vel):
        """setContactMaxCorrectingVel(vel)

        Set the maximum correcting velocity that contacts are allowed
        to generate. The default value is infinity (i.e. no
        limit). Reducing this value can help prevent "popping" of
        deeply embedded objects.

        @param vel: Maximum correcting velocity
        @type vel: float
        """
        dWorldSetContactMaxCorrectingVel(self.wid, vel)

    # getQuickStepNumIterations
    def getContactMaxCorrectingVel(self):
        """getContactMaxCorrectingVel() -> float

        Get the maximum correcting velocity that contacts are allowed
        to generate. The default value is infinity (i.e. no
        limit). Reducing this value can help prevent "popping" of
        deeply embedded objects.

        """
        return dWorldGetContactMaxCorrectingVel(self.wid)

    # setContactSurfaceLayer
    def setContactSurfaceLayer(self, depth):
        """setContactSurfaceLayer(depth)

        Set the depth of the surface layer around all geometry
        objects. Contacts are allowed to sink into the surface layer
        up to the given depth before coming to rest. The default value
        is zero. Increasing this to some small value (e.g. 0.001) can
        help prevent jittering problems due to contacts being
        repeatedly made and broken.

        @param depth: Surface layer depth
        @type depth: float
        """
        dWorldSetContactSurfaceLayer(self.wid, depth)

    # getContactSurfaceLayer
    def getContactSurfaceLayer(self):
        """getContactSurfaceLayer()

        Get the depth of the surface layer around all geometry
        objects. Contacts are allowed to sink into the surface layer
        up to the given depth before coming to rest. The default value
        is zero. Increasing this to some small value (e.g. 0.001) can
        help prevent jittering problems due to contacts being
        repeatedly made and broken.
        """
        return dWorldGetContactSurfaceLayer(self.wid)

    # setAutoDisableFlag
    def setAutoDisableFlag(self, flag):
        """setAutoDisableFlag(flag)
        
        Set the default auto-disable flag for newly created bodies.

        @param flag: True = Do auto disable
        @type flag: bool
        """
        dWorldSetAutoDisableFlag(self.wid, flag)
        
    # getAutoDisableFlag
    def getAutoDisableFlag(self):
        """getAutoDisableFlag() -> bool
        
        Get the default auto-disable flag for newly created bodies.
        """
        return dWorldGetAutoDisableFlag(self.wid)

    # setAutoDisableLinearThreshold
    def setAutoDisableLinearThreshold(self, threshold):
        """setAutoDisableLinearThreshold(threshold)
        
        Set the default auto-disable linear threshold for newly created
        bodies.

        @param threshold: Linear threshold
        @type threshold: float
        """
        dWorldSetAutoDisableLinearThreshold(self.wid, threshold)

    # getAutoDisableLinearThreshold
    def getAutoDisableLinearThreshold(self):
        """getAutoDisableLinearThreshold() -> float
        
        Get the default auto-disable linear threshold for newly created
        bodies.
        """
        return dWorldGetAutoDisableLinearThreshold(self.wid)

    # setAutoDisableAngularThreshold
    def setAutoDisableAngularThreshold(self, threshold):
        """setAutoDisableAngularThreshold(threshold)
        
        Set the default auto-disable angular threshold for newly created
        bodies.

        @param threshold: Angular threshold
        @type threshold: float
        """
        dWorldSetAutoDisableAngularThreshold(self.wid, threshold)

    # getAutoDisableAngularThreshold
    def getAutoDisableAngularThreshold(self):
        """getAutoDisableAngularThreshold() -> float
        
        Get the default auto-disable angular threshold for newly created
        bodies.
        """
        return dWorldGetAutoDisableAngularThreshold(self.wid)
    
    # setAutoDisableSteps
    def setAutoDisableSteps(self, steps):
        """setAutoDisableSteps(steps)
        
        Set the default auto-disable steps for newly created bodies.

        @param steps: Auto disable steps
        @type steps: int
        """
        dWorldSetAutoDisableSteps(self.wid, steps)

    # getAutoDisableSteps
    def getAutoDisableSteps(self):
        """getAutoDisableSteps() -> int
        
        Get the default auto-disable steps for newly created bodies.
        """
        return dWorldGetAutoDisableSteps(self.wid)

    # setAutoDisableTime
    def setAutoDisableTime(self, time):
        """setAutoDisableTime(time)
        
        Set the default auto-disable time for newly created bodies.

        @param time: Auto disable time
        @type time: float
        """
        dWorldSetAutoDisableTime(self.wid, time)

    # getAutoDisableTime
    def getAutoDisableTime(self):
        """getAutoDisableTime() -> float
        
        Get the default auto-disable time for newly created bodies.
        """
        return dWorldGetAutoDisableTime(self.wid)

    # setLinearDamping
    def setLinearDamping(self, scale):
        """setLinearDamping(scale)

        Set the world's linear damping scale.
                @param scale The linear damping scale that is to be applied to bodies.
                Default is 0 (no damping). Should be in the interval [0, 1].
        @type scale: float
        """
        dWorldSetLinearDamping(self.wid, scale)

    # getLinearDamping
    def getLinearDamping(self):
        """getLinearDamping() -> float

        Get the world's linear damping scale.
        """
        return dWorldGetLinearDamping(self.wid)

    # setAngularDamping
    def setAngularDamping(self, scale):
        """setAngularDamping(scale)

        Set the world's angular damping scale.
                @param scale The angular damping scale that is to be applied to bodies.
                Default is 0 (no damping). Should be in the interval [0, 1].
        @type scale: float
        """
        dWorldSetAngularDamping(self.wid, scale)

    # getAngularDamping
    def getAngularDamping(self):
        """getAngularDamping() -> float

        Get the world's angular damping scale.
        """
        return dWorldGetAngularDamping(self.wid)

    # impulseToForce
    def impulseToForce(self, stepsize, impulse):
        """impulseToForce(stepsize, impulse) -> 3-tuple

        If you want to apply a linear or angular impulse to a rigid
        body, instead of a force or a torque, then you can use this
        function to convert the desired impulse into a force/torque
        vector before calling the dBodyAdd... function.

        @param stepsize: Time step
        @param impulse: Impulse vector
        @type stepsize: float
        @type impulse: 3-tuple of floats
        """
        cdef dVector3 force
        dWorldImpulseToForce(self.wid, stepsize,
                             impulse[0], impulse[1], impulse[2], force)
        return force[0], force[1], force[2]

    # createBody
#    def createBody(self):
#        return Body(self)

    # createBallJoint
#    def createBallJoint(self, jointgroup=None):
#        return BallJoint(self, jointgroup)

    # createHingeJoint
#    def createHingeJoint(self, jointgroup=None):
#        return HingeJoint(self, jointgroup)

    # createHinge2Joint
#    def createHinge2Joint(self, jointgroup=None):
#        return Hinge2Joint(self, jointgroup)

    # createSliderJoint
#    def createSliderJoint(self, jointgroup=None):
#        return SliderJoint(self, jointgroup)

    # createFixedJoint
#    def createFixedJoint(self, jointgroup=None):
#        return FixedJoint(self, jointgroup)

    # createContactJoint
#    def createContactJoint(self, jointgroup, contact):
#        return ContactJoint(self, jointgroup, contact)


# Body
cdef class Body:
    """The rigid body class encapsulating the ODE body.

    This class represents a rigid body that has a location and orientation
    in space and that stores the mass properties of an object.

    When creating a Body object you have to pass the world it belongs to
    as argument to the constructor::

      >>> import ode
      >>> w = ode.World()
      >>> b = ode.Body(w)
    """

    cdef dBodyID bid
    # A reference to the world so that the world won't be destroyed while
    # there are still joints using it.
    cdef object world
    
    # A dictionary with user attributes
    # (set via __getattr__ and __setattr__)
    cdef object userattribs

    def __cinit__(self, World world not None):
        self.bid = dBodyCreate(world.wid)

    def __init__(self, World world not None):
        """Constructor.

        @param world: The world in which the body should be created.
        @type world: World
        """
        self.world = world
        self.userattribs = {}

    def __dealloc__(self):
        if self.bid != NULL:
            dBodyDestroy(self.bid)

    def __getattr__(self, name):
        try:
            return self.userattribs[name]
        except:
            raise AttributeError("Body object has no attribute '%s'" % name)
            
    def __setattr__(self, name, value):
        self.userattribs[name] = value

    def __delattr__(self, name):
        try:
            del self.userattribs[name]
        except:
            raise AttributeError("Body object has no attribute '%s'" % name)

    # setPosition
    def setPosition(self, pos):
        """setPosition(pos)

        Set the position of the body.

        @param pos: The new position
        @type pos: 3-sequence of floats
        """
        dBodySetPosition(self.bid, pos[0], pos[1], pos[2])

    # getPosition
    def getPosition(self):
        """getPosition() -> 3-tuple

        Return the current position of the body.
        """
        cdef dReal* p
        # The "const" in the original return value is cast away
        p = <dReal*>dBodyGetPosition(self.bid)
        return p[0], p[1], p[2]

    # setRotation
    def setRotation(self, R):
        """setRotation(R)

        Set the orientation of the body. The rotation matrix must be
        given as a sequence of 9 floats which are the elements of the
        matrix in row-major order.

        @param R: Rotation matrix
        @type R: 9-sequence of floats
        """
        cdef dMatrix3 m
        m[0] = R[0]
        m[1] = R[1]
        m[2] = R[2]
        m[3] = 0
        m[4] = R[3]
        m[5] = R[4]
        m[6] = R[5]
        m[7] = 0
        m[8] = R[6]
        m[9] = R[7]
        m[10] = R[8]
        m[11] = 0
        dBodySetRotation(self.bid, m)

    # getRotation
    def getRotation(self):
        """getRotation() -> 9-tuple

        Return the current rotation matrix as a tuple of 9 floats (row-major
        order).
        """
        cdef dReal* m
        # The "const" in the original return value is cast away
        m = <dReal*>dBodyGetRotation(self.bid)
        return m[0], m[1], m[2], m[4], m[5], m[6], m[8], m[9], m[10]

    # getQuaternion
    def getQuaternion(self):
        """getQuaternion() -> 4-tuple

        Return the current rotation as a quaternion. The return value
        is a list of 4 floats.
        """
        cdef dReal* q
        q = <dReal*>dBodyGetQuaternion(self.bid)
        return q[0], q[1], q[2], q[3]

    # setQuaternion
    def setQuaternion(self, q):
        """setQuaternion(q)

        Set the orientation of the body. The quaternion must be given as a
        sequence of 4 floats.

        @param q: Quaternion
        @type q: 4-sequence of floats
        """
        cdef dQuaternion w
        w[0] = q[0]
        w[1] = q[1]
        w[2] = q[2]
        w[3] = q[3]
        dBodySetQuaternion(self.bid, w)

    # setLinearVel
    def setLinearVel(self, vel):
        """setLinearVel(vel)

        Set the linear velocity of the body.

        @param vel: New velocity
        @type vel: 3-sequence of floats
        """
        dBodySetLinearVel(self.bid, vel[0], vel[1], vel[2])

    # getLinearVel
    def getLinearVel(self):
        """getLinearVel() -> 3-tuple

        Get the current linear velocity of the body.
        """
        cdef dReal* p
        # The "const" in the original return value is cast away
        p = <dReal*>dBodyGetLinearVel(self.bid)
        return p[0], p[1], p[2]

    # setAngularVel
    def setAngularVel(self, vel):
        """setAngularVel(vel)

        Set the angular velocity of the body.

        @param vel: New angular velocity
        @type vel: 3-sequence of floats
        """
        dBodySetAngularVel(self.bid, vel[0], vel[1], vel[2])

    # getAngularVel
    def getAngularVel(self):
        """getAngularVel() -> 3-tuple

        Get the current angular velocity of the body.
        """
        cdef dReal* p
        # The "const" in the original return value is cast away
        p = <dReal*>dBodyGetAngularVel(self.bid)
        return p[0], p[1], p[2]
    
    # setMass
    def setMass(self, Mass mass):
        """setMass(mass)

        Set the mass properties of the body. The argument mass must be
        an instance of a Mass object.

        @param mass: Mass properties
        @type mass: Mass
        """
        dBodySetMass(self.bid, &mass._mass)

    # getMass
    def getMass(self):
        """getMass() -> mass

        Return the mass properties as a Mass object.
        """
        cdef Mass m
        m = Mass()
        dBodyGetMass(self.bid, &m._mass)
        return m

    # addForce
    def addForce(self, f):
        """addForce(f)

        Add an external force f given in absolute coordinates. The force
        is applied at the center of mass.

        @param f: Force
        @type f: 3-sequence of floats
        """
        dBodyAddForce(self.bid, f[0], f[1], f[2])

    # addTorque
    def addTorque(self, t):
        """addTorque(t)

        Add an external torque t given in absolute coordinates.

        @param t: Torque
        @type t: 3-sequence of floats
        """
        dBodyAddTorque(self.bid, t[0], t[1], t[2])

    # addRelForce
    def addRelForce(self, f):
        """addRelForce(f)

        Add an external force f given in relative coordinates
        (relative to the body's own frame of reference). The force
        is applied at the center of mass.

        @param f: Force
        @type f: 3-sequence of floats
        """
        dBodyAddRelForce(self.bid, f[0], f[1], f[2])

    # addRelTorque
    def addRelTorque(self, t):
        """addRelTorque(t)

        Add an external torque t given in relative coordinates
        (relative to the body's own frame of reference).

        @param t: Torque
        @type t: 3-sequence of floats
        """
        dBodyAddRelTorque(self.bid, t[0], t[1], t[2])

    # addForceAtPos
    def addForceAtPos(self, f, p):
        """addForceAtPos(f, p)

        Add an external force f at position p. Both arguments must be
        given in absolute coordinates.

        @param f: Force
        @param p: Position
        @type f: 3-sequence of floats
        @type p: 3-sequence of floats
        """
        dBodyAddForceAtPos(self.bid, f[0], f[1], f[2], p[0], p[1], p[2])

    # addForceAtRelPos
    def addForceAtRelPos(self, f, p):
        """addForceAtRelPos(f, p)

        Add an external force f at position p. f is given in absolute
        coordinates and p in absolute coordinates.

        @param f: Force
        @param p: Position
        @type f: 3-sequence of floats
        @type p: 3-sequence of floats
        """
        dBodyAddForceAtRelPos(self.bid, f[0], f[1], f[2], p[0], p[1], p[2])

    # addRelForceAtPos
    def addRelForceAtPos(self, f, p):
        """addRelForceAtPos(f, p)

        Add an external force f at position p. f is given in relative
        coordinates and p in relative coordinates.

        @param f: Force
        @param p: Position
        @type f: 3-sequence of floats
        @type p: 3-sequence of floats
        """
        dBodyAddRelForceAtPos(self.bid, f[0], f[1], f[2], p[0], p[1], p[2])

    # addRelForceAtRelPos
    def addRelForceAtRelPos(self, f, p):
        """addRelForceAtRelPos(f, p)

        Add an external force f at position p. Both arguments must be
        given in relative coordinates.

        @param f: Force
        @param p: Position
        @type f: 3-sequence of floats
        @type p: 3-sequence of floats
        """
        dBodyAddRelForceAtRelPos(self.bid, f[0], f[1], f[2], p[0], p[1], p[2])

    # getForce
    def getForce(self):
        """getForce() -> 3-tuple

        Return the current accumulated force.
        """
        cdef dReal* f
        # The "const" in the original return value is cast away
        f = <dReal*>dBodyGetForce(self.bid)
        return f[0], f[1], f[2]

    # getTorque
    def getTorque(self):
        """getTorque() -> 3-tuple

        Return the current accumulated torque.
        """
        cdef dReal* f
        # The "const" in the original return value is cast away
        f = <dReal*>dBodyGetTorque(self.bid)
        return f[0], f[1], f[2]

    # setForce
    def setForce(self, f):
        """setForce(f)

        Set the body force accumulation vector.

        @param f: Force
        @type f: 3-tuple of floats
        """
        dBodySetForce(self.bid, f[0], f[1], f[2])

    # setTorque
    def setTorque(self, t):
        """setTorque(t)

        Set the body torque accumulation vector.

        @param t: Torque
        @type t: 3-tuple of floats
        """
        dBodySetTorque(self.bid, t[0], t[1], t[2])

    # getRelPointPos
    def getRelPointPos(self, p):
        """getRelPointPos(p) -> 3-tuple

        Utility function that takes a point p on a body and returns
        that point's position in global coordinates. The point p
        must be given in body relative coordinates.

        @param p: Body point (local coordinates)
        @type p: 3-sequence of floats
        """

        cdef dVector3 res
        dBodyGetRelPointPos(self.bid, p[0], p[1], p[2], res)
        return res[0], res[1], res[2]

    # getRelPointVel
    def getRelPointVel(self, p):
        """getRelPointVel(p) -> 3-tuple

        Utility function that takes a point p on a body and returns
        that point's velocity in global coordinates. The point p
        must be given in body relative coordinates.

        @param p: Body point (local coordinates)
        @type p: 3-sequence of floats
        """
        cdef dVector3 res
        dBodyGetRelPointVel(self.bid, p[0], p[1], p[2], res)
        return res[0], res[1], res[2]

    # getPointVel
    def getPointVel(self, p):
        """getPointVel(p) -> 3-tuple

        Utility function that takes a point p on a body and returns
        that point's velocity in global coordinates. The point p
        must be given in global coordinates.

        @param p: Body point (global coordinates)
        @type p: 3-sequence of floats
        """
        cdef dVector3 res
        dBodyGetPointVel(self.bid, p[0], p[1], p[2], res)
        return res[0], res[1], res[2]

    # getPosRelPoint
    def getPosRelPoint(self, p):
        """getPosRelPoint(p) -> 3-tuple

        This is the inverse of getRelPointPos(). It takes a point p in
        global coordinates and returns the point's position in
        body-relative coordinates.

        @param p: Body point (global coordinates)
        @type p: 3-sequence of floats
        """
        cdef dVector3 res
        dBodyGetPosRelPoint(self.bid, p[0], p[1], p[2], res)
        return res[0], res[1], res[2]

    # vectorToWorld
    def vectorToWorld(self, v):
        """vectorToWorld(v) -> 3-tuple

        Given a vector v expressed in the body coordinate system, rotate
        it to the world coordinate system.

        @param v: Vector in body coordinate system
        @type v: 3-sequence of floats
        """
        cdef dVector3 res
        dBodyVectorToWorld(self.bid, v[0], v[1], v[2], res)
        return res[0], res[1], res[2]

    # vectorFromWorld
    def vectorFromWorld(self, v):
        """vectorFromWorld(v) -> 3-tuple

        Given a vector v expressed in the world coordinate system, rotate
        it to the body coordinate system.

        @param v: Vector in world coordinate system
        @type v: 3-sequence of floats
        """
        cdef dVector3 res
        dBodyVectorFromWorld(self.bid, v[0], v[1], v[2], res)
        return res[0], res[1], res[2]

    # Enable
    def enable(self):
        """enable()

        Manually enable a body.
        """
        dBodyEnable(self.bid)
        
    # Disable
    def disable(self):
        """disable()

        Manually disable a body. Note that a disabled body that is connected
        through a joint to an enabled body will be automatically re-enabled
        at the next simulation step.
        """
        dBodyDisable(self.bid)
        
    # isEnabled
    def isEnabled(self):
        """isEnabled() -> bool

        Check if a body is currently enabled.
        """
        return dBodyIsEnabled(self.bid)
        
    # setFiniteRotationMode
    def setFiniteRotationMode(self, mode):
        """setFiniteRotationMode(mode)

        This function controls the way a body's orientation is updated at
        each time step. The mode argument can be:
        
         - 0: An "infinitesimal" orientation update is used. This is
           fast to compute, but it can occasionally cause inaccuracies
           for bodies that are rotating at high speed, especially when
           those bodies are joined to other bodies. This is the default
           for every new body that is created.
        
         - 1: A "finite" orientation update is used. This is more
           costly to compute, but will be more accurate for high speed
           rotations. Note however that high speed rotations can result
           in many types of error in a simulation, and this mode will
           only fix one of those sources of error.

        @param mode: Rotation mode (0/1)
        @type mode: int
        """
        dBodySetFiniteRotationMode(self.bid, mode)
        
    # getFiniteRotationMode
    def getFiniteRotationMode(self):
        """getFiniteRotationMode() -> mode (0/1)

        Return the current finite rotation mode of a body (0 or 1).
        See setFiniteRotationMode().
        """
        return dBodyGetFiniteRotationMode(self.bid)

    # setFiniteRotationAxis
    def setFiniteRotationAxis(self, a):
        """setFiniteRotationAxis(a)

        Set the finite rotation axis of the body.  This axis only has a
        meaning when the finite rotation mode is set
        (see setFiniteRotationMode()).
        
        @param a: Axis
        @type a: 3-sequence of floats
        """
        dBodySetFiniteRotationAxis(self.bid, a[0], a[1], a[2])

    # getFiniteRotationAxis
    def getFiniteRotationAxis(self):
        """getFiniteRotationAxis() -> 3-tuple

        Return the current finite rotation axis of the body.
        """
        cdef dVector3 p
        # The "const" in the original return value is cast away
        dBodyGetFiniteRotationAxis(self.bid, p)
        return p[0], p[1], p[2]
        
    # getNumJoints
    def getNumJoints(self):
        """getNumJoints() -> int

        Return the number of joints that are attached to this body.
        """
        return dBodyGetNumJoints(self.bid)

    # setGravityMode
    def setGravityMode(self, mode):
        """setGravityMode(mode)

        Set whether the body is influenced by the world's gravity
        or not. If mode is True it is, otherwise it isn't.
        Newly created bodies are always influenced by the world's gravity.

        @param mode: Gravity mode
        @type mode: bool
        """
        dBodySetGravityMode(self.bid, mode)
        
    # getGravityMode
    def getGravityMode(self):
        """getGravityMode() -> bool

        Return True if the body is influenced by the world's gravity.
        """
        return dBodyGetGravityMode(self.bid)

    def setDynamic(self):
        """setDynamic()

        Set a body to the (default) "dynamic" state, instead of "kinematic".
        See setKinematic() for more information.
        """
        dBodySetDynamic(self.bid)

    def setKinematic(self):
        """setKinematic()

        Set the kinematic state of the body (change it into a kinematic body)

        Kinematic bodies behave as if they had infinite mass. This means they don't react
        to any force (gravity, constraints or user-supplied); they simply follow 
        velocity to reach the next position. [from ODE wiki]

        """
        dBodySetKinematic(self.bid)

    def isKinematic(self):
        """isKinematic() -> bool

        Return True if the body is kinematic (not influenced by other forces).

        Kinematic bodies behave as if they had infinite mass. This means they don't react
        to any force (gravity, constraints or user-supplied); they simply follow
        velocity to reach the next position. [from ODE wiki]

        """
        return dBodyIsKinematic(self.bid)

    def setMaxAngularSpeed(self, max_speed):
        """setMaxAngularSpeed(max_speed)

        You can also limit the maximum angular speed. In contrast to the damping
        functions, the angular velocity is affected before the body is moved.
        This means that it will introduce errors in joints that are forcing the
        body to rotate too fast. Some bodies have naturally high angular
        velocities (like cars' wheels), so you may want to give them a very high
        (like the default, dInfinity) limit.

        """
        dBodySetMaxAngularSpeed(self.bid, max_speed)


# JointGroup
cdef class JointGroup:
    """Joint group.

    Constructor::
    
      JointGroup()
    """

    # JointGroup ID
    cdef dJointGroupID gid
    # A list of Python joints that were added to the group
    cdef object jointlist

    def __cinit__(self):
        self.gid = dJointGroupCreate(0)

    def __init__(self):
        self.jointlist = []

    def __dealloc__(self):
        if self.gid != NULL:
            for j in self.jointlist:
                j._destroyed()
            dJointGroupDestroy(self.gid)

    # empty
    def empty(self):
        """empty()

        Destroy all joints in the group.
        """
        dJointGroupEmpty(self.gid)
        for j in self.jointlist:
            j._destroyed()
        self.jointlist = []

    def _addjoint(self, j):
        """_addjoint(j)

        Add a joint to the group.  This is an internal method that is
        called by the joints.  The group has to know the Python
        wrappers because it has to notify them when the group is
        emptied (so that the ODE joints won't get destroyed
        twice). The notification is done by calling _destroyed() on
        the Python joints.

        @param j: The joint to add
        @type j: Joint
        """
        self.jointlist.append(j)


######################################################################

# Joint
cdef class Joint:
    """Base class for all joint classes."""

    # Joint id as returned by dJointCreateXxx()
    cdef dJointID jid
    # A reference to the world so that the world won't be destroyed while
    # there are still joints using it.
    cdef object world
    # The feedback buffer
    cdef dJointFeedback* feedback

    cdef object body1
    cdef object body2

    # A dictionary with user attributes
    # (set via __getattr__ and __setattr__)
    cdef object userattribs

    def __cinit__(self, *a, **kw):
        self.jid = NULL
        self.world = None
        self.feedback = NULL
        self.body1 = None
        self.body2 = None
        self.userattribs = {}

    def __init__(self, *a, **kw):
        raise NotImplementedError("Joint base class can't be used directly")

    def __dealloc__(self):
        self.setFeedback(False)
        if self.jid != NULL:
            dJointDestroy(self.jid)

    def __getattr__(self, name):
        try:
            return self.userattribs[name]
        except:
            raise AttributeError("Joint object has no attribute '%s'" % name)
            
    def __setattr__(self, name, value):
        self.userattribs[name] = value

    def __delattr__(self, name):
        try:
            del self.userattribs[name]
        except:
            raise AttributeError("Joint object has no attribute '%s'" % name)

    # _destroyed
    def _destroyed(self):
        """Notify the joint object about an external destruction of the ODE joint.

        This method has to be called when the underlying ODE object
        was destroyed by someone else (e.g. by a joint group). The Python
        wrapper will then refrain from destroying it again.
        """
        self.jid = NULL

    # enable
    def enable(self):
        """enable()

        Enable the joint. Disabled joints are completely ignored during the
        simulation. Disabled joints don't lose the already computed information
        like anchors and axes.
        """
        dJointEnable(self.jid)

    # disable
    def disable(self):
        """disable()

        Disable the joint. Disabled joints are completely ignored during the
        simulation. Disabled joints don't lose the already computed information
        like anchors and axes.
        """
        dJointDisable(self.jid)

    # isEnabled
    def isEnabled(self):
        """isEnabled() -> bool

        Determine whether the joint is enabled. Disabled joints are completely
        ignored during the simulation. Disabled joints don't lose the already
        computed information like anchors and axes.
        """
        return dJointIsEnabled(self.jid)

    # attach
    def attach(self, Body body1, Body body2):
        """attach(body1, body2)

        Attach the joint to some new bodies. A body can be attached
        to the environment by passing None as second body.
        
        @param body1: First body
        @param body2: Second body
        @type body1: Body
        @type body2: Body
        """
        cdef dBodyID id1, id2

        if body1 == None:
            id1 = NULL
        else:
            id1 = body1.bid
            
        if body2 == None:
            id2 = NULL
        else:
            id2 = body2.bid

        self.body1 = body1
        self.body2 = body2
        dJointAttach(self.jid, id1, id2)

    # getBody
    def getBody(self, index):
        """getBody(index) -> Body

        Return the bodies that this joint connects. If index is 0 the
        "first" body will be returned, corresponding to the body1
        argument of the attach() method. If index is 1 the "second" body
        will be returned, corresponding to the body2 argument of the
        attach() method.

        @param index: Bodx index (0 or 1).
        @type index: int
        """
        
        if index == 0:
            return self.body1
        elif index == 1:
            return self.body2
        else:
            raise IndexError()

    # setFeedback
    def setFeedback(self, flag=1):
        """setFeedback(flag=True)

        Create a feedback buffer. If flag is True then a buffer is
        allocated and the forces/torques applied by the joint can
        be read using the getFeedback() method. If flag is False the
        buffer is released.

        @param flag: Specifies whether a buffer should be created or released
        @type flag: bool
        """
        
        if flag:
            # Was there already a buffer allocated? then we're finished
            if self.feedback != NULL:
                return
            # Allocate a buffer and pass it to ODE
            self.feedback = <dJointFeedback*>malloc(sizeof(dJointFeedback))
            if self.feedback == NULL:
                raise MemoryError("can't allocate feedback buffer")
            dJointSetFeedback(self.jid, self.feedback)
        else:
            if self.feedback != NULL:
                # Free a previously allocated buffer
                dJointSetFeedback(self.jid, NULL)
                free(self.feedback)
                self.feedback = NULL
        
    # getFeedback
    def getFeedback(self):
        """getFeedback() -> (force1, torque1, force2, torque2)

        Get the forces/torques applied by the joint. If feedback is
        activated (i.e. setFeedback(True) was called) then this method
        returns a tuple (force1, torque1, force2, torque2) with the
        forces and torques applied to body 1 and body 2.  The
        forces/torques are given as 3-tuples.

        If feedback is deactivated then the method always returns None.
        """
        cdef dJointFeedback* fb
        
        fb = dJointGetFeedback(self.jid)
        if fb == NULL:
            return None
           
        f1 = (fb.f1[0], fb.f1[1], fb.f1[2])
        t1 = (fb.t1[0], fb.t1[1], fb.t1[2])
        f2 = (fb.f2[0], fb.f2[1], fb.f2[2])
        t2 = (fb.t2[0], fb.t2[1], fb.t2[2])
        return f1, t1, f2, t2

######################################################################


# BallJoint
cdef class BallJoint(Joint):
    """Ball joint.

    Constructor::
    
      BallJoint(world, jointgroup=None)
    """

    def __cinit__(self, World world not None, jointgroup=None):
        cdef JointGroup jg
        cdef dJointGroupID jgid

        jgid = NULL
        if jointgroup != None:
            jg = jointgroup
            jgid = jg.gid
        self.jid = dJointCreateBall(world.wid, jgid)

    def __init__(self, World world not None, jointgroup=None):
        self.world = world
        if jointgroup != None:
            jointgroup._addjoint(self)
            
    # setAnchor
    def setAnchor(self, pos):
        """setAnchor(pos)

        Set the joint anchor point which must be specified in world
        coordinates.

        @param pos: Anchor position
        @type pos: 3-sequence of floats
        """
        dJointSetBallAnchor(self.jid, pos[0], pos[1], pos[2])
    
    # getAnchor
    def getAnchor(self):
        """getAnchor() -> 3-tuple of floats

        Get the joint anchor point, in world coordinates.  This
        returns the point on body 1.  If the joint is perfectly
        satisfied, this will be the same as the point on body 2.
        """
        
        cdef dVector3 p
        dJointGetBallAnchor(self.jid, p)
        return p[0], p[1], p[2]

    # getAnchor2
    def getAnchor2(self):
        """getAnchor2() -> 3-tuple of floats

        Get the joint anchor point, in world coordinates.  This
        returns the point on body 2. If the joint is perfectly
        satisfied, this will be the same as the point on body 1.
        """

        cdef dVector3 p
        dJointGetBallAnchor2(self.jid, p)
        return p[0], p[1], p[2]

    # setParam
    def setParam(self, param, value):
        pass

    # getParam
    def getParam(self, param):
        return 0.0
        
    
# HingeJoint
cdef class HingeJoint(Joint):
    """Hinge joint.

    Constructor::
    
      HingeJoint(world, jointgroup=None)
    """

    def __cinit__(self, World world not None, jointgroup=None):
        cdef JointGroup jg
        cdef dJointGroupID jgid
        
        jgid = NULL
        if jointgroup != None:
            jg = jointgroup
            jgid = jg.gid
        self.jid = dJointCreateHinge(world.wid, jgid)
        
    def __init__(self, World world not None, jointgroup=None):
        self.world = world
        if jointgroup != None:
            jointgroup._addjoint(self)

    # setAnchor
    def setAnchor(self, pos):
        """setAnchor(pos)

        Set the hinge anchor which must be given in world coordinates.

        @param pos: Anchor position
        @type pos: 3-sequence of floats
        """
        dJointSetHingeAnchor(self.jid, pos[0], pos[1], pos[2])
    
    # getAnchor
    def getAnchor(self):
        """getAnchor() -> 3-tuple of floats

        Get the joint anchor point, in world coordinates. This returns
        the point on body 1. If the joint is perfectly satisfied, this
        will be the same as the point on body 2.
        """
        cdef dVector3 p
        dJointGetHingeAnchor(self.jid, p)
        return p[0], p[1], p[2]

    # getAnchor2
    def getAnchor2(self):
        """getAnchor2() -> 3-tuple of floats

        Get the joint anchor point, in world coordinates. This returns
        the point on body 2. If the joint is perfectly satisfied, this
        will be the same as the point on body 1.
        """
        cdef dVector3 p
        dJointGetHingeAnchor2(self.jid, p)
        return p[0], p[1], p[2]

    # setAxis
    def setAxis(self, axis):
        """setAxis(axis)

        Set the hinge axis.

        @param axis: Hinge axis
        @type axis: 3-sequence of floats
        """
        dJointSetHingeAxis(self.jid, axis[0], axis[1], axis[2])
    
    # getAxis
    def getAxis(self):
        """getAxis() -> 3-tuple of floats

        Get the hinge axis.
        """
        cdef dVector3 a
        dJointGetHingeAxis(self.jid, a)
        return a[0], a[1], a[2]

    # getAngle
    def getAngle(self):
        """getAngle() -> float

        Get the hinge angle. The angle is measured between the two
        bodies, or between the body and the static environment. The
        angle will be between -pi..pi.

        When the hinge anchor or axis is set, the current position of
        the attached bodies is examined and that position will be the
        zero angle.
        """
        
        return dJointGetHingeAngle(self.jid)

    # getAngleRate
    def getAngleRate(self):
        """getAngleRate() -> float

        Get the time derivative of the angle.
        """
        return dJointGetHingeAngleRate(self.jid)

    # addTorque
    def addTorque(self, torque):
        """addTorque(torque)

        Applies the torque about the hinge axis.

        @param torque: Torque magnitude
        @type torque: float
        """
        dJointAddHingeTorque(self.jid, torque)

    # setParam
    def setParam(self, param, value):
        """setParam(param, value)

        Set limit/motor parameters for the joint.

        param is one of ParamLoStop, ParamHiStop, ParamVel, ParamFMax,
        ParamFudgeFactor, ParamBounce, ParamCFM, ParamStopERP, ParamStopCFM,
        ParamSuspensionERP, ParamSuspensionCFM.

        These parameter names can be optionally followed by a digit (2
        or 3) to indicate the second or third set of parameters.

        @param param: Selects the parameter to set
        @param value: Parameter value
        @type param: int
        @type value: float
        """
        
        dJointSetHingeParam(self.jid, param, value)

    # getParam
    def getParam(self, param):
        """getParam(param) -> float

        Get limit/motor parameters for the joint.

        param is one of ParamLoStop, ParamHiStop, ParamVel, ParamFMax,
        ParamFudgeFactor, ParamBounce, ParamCFM, ParamStopERP, ParamStopCFM,
        ParamSuspensionERP, ParamSuspensionCFM.

        These parameter names can be optionally followed by a digit (2
        or 3) to indicate the second or third set of parameters.

        @param param: Selects the parameter to read
        @type param: int
        """
        return dJointGetHingeParam(self.jid, param)
        
        
# SliderJoint
cdef class SliderJoint(Joint):
    """Slider joint.
    
    Constructor::
    
      SlideJoint(world, jointgroup=None)
    """

    def __cinit__(self, World world not None, jointgroup=None):
        cdef JointGroup jg
        cdef dJointGroupID jgid

        jgid = NULL
        if jointgroup != None:
            jg = jointgroup
            jgid = jg.gid
        self.jid = dJointCreateSlider(world.wid, jgid)

    def __init__(self, World world not None, jointgroup=None):
        self.world = world
        if jointgroup != None:
            jointgroup._addjoint(self)
          
    # setAxis
    def setAxis(self, axis):
        """setAxis(axis)

        Set the slider axis parameter.

        @param axis: Slider axis
        @type axis: 3-sequence of floats
        """
        dJointSetSliderAxis(self.jid, axis[0], axis[1], axis[2])
    
    # getAxis
    def getAxis(self):
        """getAxis() -> 3-tuple of floats

        Get the slider axis parameter.
        """
        cdef dVector3 a
        dJointGetSliderAxis(self.jid, a)
        return a[0], a[1], a[2]

    # getPosition
    def getPosition(self):
        """getPosition() -> float

        Get the slider linear position (i.e. the slider's "extension").

        When the axis is set, the current position of the attached
        bodies is examined and that position will be the zero
        position.
        """
        
        return dJointGetSliderPosition(self.jid)

    # getPositionRate
    def getPositionRate(self):
        """getPositionRate() -> float

        Get the time derivative of the position.
        """
        return dJointGetSliderPositionRate(self.jid)

    # addForce
    def addForce(self, force):
        """addForce(force)

        Applies the given force in the slider's direction.

        @param force: Force magnitude
        @type force: float
        """
        dJointAddSliderForce(self.jid, force)

    # setParam
    def setParam(self, param, value):
        dJointSetSliderParam(self.jid, param, value)

    # getParam
    def getParam(self, param):
        return dJointGetSliderParam(self.jid, param)
        
    
# UniversalJoint
cdef class UniversalJoint(Joint):
    """Universal joint.

    Constructor::
    
      UniversalJoint(world, jointgroup=None)
    """

    def __cinit__(self, World world not None, jointgroup=None):
        cdef JointGroup jg
        cdef dJointGroupID jgid

        jgid = NULL
        if jointgroup != None:
            jg = jointgroup
            jgid = jg.gid
        self.jid = dJointCreateUniversal(world.wid, jgid)

    def __init__(self, World world not None, jointgroup=None):
        self.world = world
        if jointgroup != None:
            jointgroup._addjoint(self)

    # setAnchor
    def setAnchor(self, pos):
        """setAnchor(pos)

        Set the universal anchor.

        @param pos: Anchor position
        @type pos: 3-sequence of floats
        """
        dJointSetUniversalAnchor(self.jid, pos[0], pos[1], pos[2])
    
    # getAnchor
    def getAnchor(self):
        """getAnchor() -> 3-tuple of floats

        Get the joint anchor point, in world coordinates. This returns
        the point on body 1. If the joint is perfectly satisfied, this
        will be the same as the point on body 2.
        """
        
        cdef dVector3 p
        dJointGetUniversalAnchor(self.jid, p)
        return p[0], p[1], p[2]

    # getAnchor2
    def getAnchor2(self):
        """getAnchor2() -> 3-tuple of floats

        Get the joint anchor point, in world coordinates. This returns
        the point on body 2. If the joint is perfectly satisfied, this
        will be the same as the point on body 1.
        """
        
        cdef dVector3 p
        dJointGetUniversalAnchor2(self.jid, p)
        return p[0], p[1], p[2]

    # setAxis1
    def setAxis1(self, axis):
        """setAxis1(axis)

        Set the first universal axis. Axis 1 and axis 2 should be
        perpendicular to each other.

        @param axis: Joint axis
        @type axis: 3-sequence of floats
        """
        dJointSetUniversalAxis1(self.jid, axis[0], axis[1], axis[2])
    
    # getAxis1
    def getAxis1(self):
        """getAxis1() -> 3-tuple of floats

        Get the first univeral axis.
        """
        cdef dVector3 a
        dJointGetUniversalAxis1(self.jid, a)
        return a[0], a[1], a[2]

    # setAxis2
    def setAxis2(self, axis):
        """setAxis2(axis)

        Set the second universal axis. Axis 1 and axis 2 should be
        perpendicular to each other.

        @param axis: Joint axis
        @type axis: 3-sequence of floats
        """
        dJointSetUniversalAxis2(self.jid, axis[0], axis[1], axis[2])
    
    # getAxis2
    def getAxis2(self):
        """getAxis2() -> 3-tuple of floats

        Get the second univeral axis.
        """
        cdef dVector3 a
        dJointGetUniversalAxis2(self.jid, a)
        return a[0], a[1], a[2]

    # addTorques
    def addTorques(self, torque1, torque2):
        """addTorques(torque1, torque2)

        Applies torque1 about axis 1, and torque2 about axis 2.

        @param torque1: Torque 1 magnitude
        @param torque2: Torque 2 magnitude
        @type torque1: float
        @type torque2: float
        """
        dJointAddUniversalTorques(self.jid, torque1, torque2)

    def getAngle1(self):
        return dJointGetUniversalAngle1(self.jid)

    def getAngle2(self):
        return dJointGetUniversalAngle2(self.jid)
    
    def getAngle1Rate(self):
        return dJointGetUniversalAngle1Rate(self.jid)

    def getAngle2Rate(self):
        return dJointGetUniversalAngle2Rate(self.jid)

    # setParam
    def setParam(self, param, value):
        dJointSetUniversalParam(self.jid, param, value)

    # getParam
    def getParam(self, param):
        return dJointGetUniversalParam(self.jid, param)

    
# Hinge2Joint
cdef class Hinge2Joint(Joint):
    """Hinge2 joint.

    Constructor::
    
      Hinge2Joint(world, jointgroup=None)
    """

    def __cinit__(self, World world not None, jointgroup=None):
        cdef JointGroup jg
        cdef dJointGroupID jgid

        jgid = NULL
        if jointgroup != None:
            jg = jointgroup
            jgid = jg.gid
        self.jid = dJointCreateHinge2(world.wid, jgid)

    def __init__(self, World world, jointgroup=None):
        self.world = world
        if jointgroup != None:
            jointgroup._addjoint(self)

    # setAnchor
    def setAnchor(self, pos):
        """setAnchor(pos)

        Set the hinge-2 anchor.

        @param pos: Anchor position
        @type pos: 3-sequence of floats
        """
        dJointSetHinge2Anchor(self.jid, pos[0], pos[1], pos[2])
    
    # getAnchor
    def getAnchor(self):
        """getAnchor() -> 3-tuple of floats

        Get the joint anchor point, in world coordinates. This returns
        the point on body 1. If the joint is perfectly satisfied, this
        will be the same as the point on body 2.
        """
        
        cdef dVector3 p
        dJointGetHinge2Anchor(self.jid, p)
        return p[0], p[1], p[2]

    # getAnchor2
    def getAnchor2(self):
        """getAnchor2() -> 3-tuple of floats

        Get the joint anchor point, in world coordinates. This returns
        the point on body 2. If the joint is perfectly satisfied, this
        will be the same as the point on body 1.
        """
        
        cdef dVector3 p
        dJointGetHinge2Anchor2(self.jid, p)
        return p[0], p[1], p[2]

    # setAxis1
    def setAxis1(self, axis):
        """setAxis1(axis)

        Set the first hinge-2 axis. Axis 1 and axis 2 must not lie
        along the same line.

        @param axis: Joint axis
        @type axis: 3-sequence of floats
        """
        
        dJointSetHinge2Axis1(self.jid, axis[0], axis[1], axis[2])
    
    # getAxis1
    def getAxis1(self):
        """getAxis1() -> 3-tuple of floats

        Get the first hinge-2 axis.
        """
        cdef dVector3 a
        dJointGetHinge2Axis1(self.jid, a)
        return a[0], a[1], a[2]

    # setAxis2
    def setAxis2(self, axis):
        """setAxis2(axis)

        Set the second hinge-2 axis. Axis 1 and axis 2 must not lie
        along the same line.

        @param axis: Joint axis
        @type axis: 3-sequence of floats
        """
        dJointSetHinge2Axis2(self.jid, axis[0], axis[1], axis[2])
    
    # getAxis2
    def getAxis2(self):
        """getAxis2() -> 3-tuple of floats

        Get the second hinge-2 axis.
        """
        cdef dVector3 a
        dJointGetHinge2Axis2(self.jid, a)
        return a[0], a[1], a[2]

    # getAngle
    def getAngle1(self):
        """getAngle1() -> float

        Get the first hinge-2 angle (around axis 1).

        When the anchor or axis is set, the current position of the
        attached bodies is examined and that position will be the zero
        angle.
        """
        return dJointGetHinge2Angle1(self.jid)

    # getAngle1Rate
    def getAngle1Rate(self):
        """getAngle1Rate() -> float

        Get the time derivative of the first hinge-2 angle.
        """
        return dJointGetHinge2Angle1Rate(self.jid)

    # getAngle2Rate
    def getAngle2Rate(self):
        """getAngle2Rate() -> float

        Get the time derivative of the second hinge-2 angle.
        """
        return dJointGetHinge2Angle2Rate(self.jid)

    # addTorques
    def addTorques(self, torque1, torque2):
        """addTorques(torque1, torque2)

        Applies torque1 about axis 1, and torque2 about axis 2.

        @param torque1: Torque 1 magnitude
        @param torque2: Torque 2 magnitude
        @type torque1: float
        @type torque2: float
        """
        dJointAddHinge2Torques(self.jid, torque1, torque2)

    # setParam
    def setParam(self, param, value):
        dJointSetHinge2Param(self.jid, param, value)

    # getParam
    def getParam(self, param):
        return dJointGetHinge2Param(self.jid, param)

    
# FixedJoint
cdef class FixedJoint(Joint):
    """Fixed joint.

    Constructor::
    
      FixedJoint(world, jointgroup=None)
    """

    def __cinit__(self, World world not None, jointgroup=None):
        cdef JointGroup jg
        cdef dJointGroupID jgid

        jgid = NULL
        if jointgroup != None:
            jg = jointgroup
            jgid = jg.gid
        self.jid = dJointCreateFixed(world.wid, jgid)

    def __init__(self, World world not None, jointgroup=None):
        self.world = world
        if jointgroup != None:
            jointgroup._addjoint(self)

    # setFixed
    def setFixed(self):
        """setFixed()

        Call this on the fixed joint after it has been attached to
        remember the current desired relative offset and desired
        relative rotation between the bodies.
        """
        dJointSetFixed(self.jid)

        
# ContactJoint
cdef class ContactJoint(Joint):
    """Contact joint.

    Constructor::
    
      ContactJoint(world, jointgroup, contact)
    """

    def __cinit__(self, World world not None, jointgroup, Contact contact):
        cdef JointGroup jg
        cdef dJointGroupID jgid
        jgid = NULL
        if jointgroup != None:
            jg = jointgroup
            jgid = jg.gid
        self.jid = dJointCreateContact(world.wid, jgid, &contact._contact)

    def __init__(self, World world not None, jointgroup, Contact contact):
        self.world = world
        if jointgroup != None:
            jointgroup._addjoint(self)

# AMotor
cdef class AMotor(Joint):
    """AMotor joint.
    
    Constructor::
    
      AMotor(world, jointgroup=None)
    """

    def __cinit__(self, World world not None, jointgroup=None):
        cdef JointGroup jg
        cdef dJointGroupID jgid

        jgid = NULL
        if jointgroup != None:
            jg = jointgroup
            jgid = jg.gid
        self.jid = dJointCreateAMotor(world.wid, jgid)

    def __init__(self, World world not None, jointgroup=None):
        self.world = world
        if jointgroup != None:
            jointgroup._addjoint(self)
            
    # setMode
    def setMode(self, mode):
        """setMode(mode)

        Set the angular motor mode.  mode must be either AMotorUser or
        AMotorEuler.

        @param mode: Angular motor mode
        @type mode: int
        """
        dJointSetAMotorMode(self.jid, mode)

    # getMode
    def getMode(self):
        """getMode()

        Return the angular motor mode (AMotorUser or AMotorEuler).
        """
        return dJointGetAMotorMode(self.jid)

    # setNumAxes
    def setNumAxes(self, int num):
        """setNumAxes(num)

        Set the number of angular axes that will be controlled by the AMotor.
        num may be in the range from 0 to 3.

        @param num: Number of axes (0-3)
        @type num: int
        """
        dJointSetAMotorNumAxes(self.jid, num)

    # getNumAxes
    def getNumAxes(self):
        """getNumAxes() -> int

        Get the number of angular axes that are controlled by the AMotor.
        """
        return dJointGetAMotorNumAxes(self.jid)

    # setAxis
    def setAxis(self, int anum, int rel, axis):
        """setAxis(anum, rel, axis)

        Set an AMotor axis.

        The anum argument selects the axis to change (0,1 or 2).
        Each axis can have one of three "relative orientation" modes,
        selected by rel:
        
        0: The axis is anchored to the global frame.
        1: The axis is anchored to the first body.
        2: The axis is anchored to the second body.

        The axis vector is always specified in global coordinates
        regardless of the setting of rel.

        @param anum: Axis number
        @param rel: Relative orientation mode
        @param axis: Axis
        @type anum: int
        @type rel: int
        @type axis: 3-sequence of floats
        """
        dJointSetAMotorAxis(self.jid, anum, rel, axis[0], axis[1], axis[2])

    # getAxis
    def getAxis(self, int anum):
        """getAxis(anum)

        Get an AMotor axis.

        @param anum: Axis index (0-2)
        @type anum: int
        """
        cdef dVector3 a
        dJointGetAMotorAxis(self.jid, anum, a)
        return a[0], a[1], a[2]

    # getAxisRel
    def getAxisRel(self, int anum):
        """getAxisRel(anum) -> int

        Get the relative mode of an axis.

        @param anum: Axis index (0-2)
        @type anum: int
        """
        return dJointGetAMotorAxisRel(self.jid, anum)

    # setAngle
    def setAngle(self, int anum, angle):
        """setAngle(anum, angle)

        Tell the AMotor what the current angle is along axis anum.

        @param anum: Axis index
        @param angle: Angle
        @type anum: int
        @type angle: float
        """
        dJointSetAMotorAngle(self.jid, anum, angle)

    # getAngle
    def getAngle(self, int anum):
        """getAngle(anum) -> float

        Return the current angle for axis anum.

        @param anum: Axis index
        @type anum: int
        """
        return dJointGetAMotorAngle(self.jid, anum)

    # getAngleRate
    def getAngleRate(self, int anum):
        """getAngleRate(anum) -> float

        Return the current angle rate for axis anum.

        @param anum: Axis index
        @type anum: int
        """
        return dJointGetAMotorAngleRate(self.jid, anum)

    # addTorques
    def addTorques(self, torque0, torque1, torque2):
        """addTorques(torque0, torque1, torque2)

        Applies torques about the AMotor's axes.

        @param torque0: Torque 0 magnitude
        @param torque1: Torque 1 magnitude
        @param torque2: Torque 2 magnitude
        @type torque0: float
        @type torque1: float
        @type torque2: float
        """
        dJointAddAMotorTorques(self.jid, torque0, torque1, torque2)

    # setParam
    def setParam(self, param, value):
        dJointSetAMotorParam(self.jid, param, value)

    # getParam
    def getParam(self, param):
        return dJointGetAMotorParam(self.jid, param)


# LMotor
cdef class LMotor(Joint):
    """LMotor joint.
    
    Constructor::
    
      LMotor(world, jointgroup=None)
    """

    def __cinit__(self, World world not None, jointgroup=None):
        cdef JointGroup jg
        cdef dJointGroupID jgid

        jgid = NULL
        if jointgroup != None:
            jg = jointgroup
            jgid = jg.gid
        self.jid = dJointCreateLMotor(world.wid, jgid)

    def __init__(self, World world not None, jointgroup=None):
        self.world = world
        if jointgroup != None:
            jointgroup._addjoint(self)
            
    # setNumAxes
    def setNumAxes(self, int num):
        """setNumAxes(num)

        Set the number of angular axes that will be controlled by the LMotor.
        num may be in the range from 0 to 3.

        @param num: Number of axes (0-3)
        @type num: int
        """
        dJointSetLMotorNumAxes(self.jid, num)

    # getNumAxes
    def getNumAxes(self):
        """getNumAxes() -> int

        Get the number of angular axes that are controlled by the LMotor.
        """
        return dJointGetLMotorNumAxes(self.jid)

    # setAxis
    def setAxis(self, int anum, int rel, axis):
        """setAxis(anum, rel, axis)

        Set an LMotor axis.

        The anum argument selects the axis to change (0,1 or 2).
        Each axis can have one of three "relative orientation" modes,
        selected by rel:

        0: The axis is anchored to the global frame.
        1: The axis is anchored to the first body.
        2: The axis is anchored to the second body.

        @param anum: Axis number
        @param rel: Relative orientation mode
        @param axis: Axis
        @type anum: int
        @type rel: int
        @type axis: 3-sequence of floats
        """
        dJointSetLMotorAxis(self.jid, anum, rel, axis[0], axis[1], axis[2])

    # getAxis
    def getAxis(self, int anum):
        """getAxis(anum)

        Get an LMotor axis.

        @param anum: Axis index (0-2)
        @type anum: int
        """
        cdef dVector3 a
        dJointGetLMotorAxis(self.jid, anum, a)
        return a[0], a[1], a[2]

    # setParam
    def setParam(self, param, value):
        dJointSetLMotorParam(self.jid, param, value)

    # getParam
    def getParam(self, param):
        return dJointGetLMotorParam(self.jid, param)


# Plane2DJoint
cdef class Plane2DJoint(Joint):
    """Plane-2D Joint.

    Constructor::
    
      Plane2DJoint(world, jointgroup=None)
    """

    def __cinit__(self, World world not None, jointgroup=None):
        cdef JointGroup jg
        cdef dJointGroupID jgid

        jgid = NULL
        if jointgroup != None:
            jg = jointgroup
            jgid = jg.gid
        self.jid = dJointCreatePlane2D(world.wid, jgid)

    def __init__(self, World world not None, jointgroup=None):
        self.world = world
        if jointgroup != None:
            jointgroup._addjoint(self)
            
    def setXParam(self, param, value):
        dJointSetPlane2DXParam(self.jid, param, value)
        
    def setYParam(self, param, value):
        dJointSetPlane2DYParam(self.jid, param, value)
        
    def setAngleParam(self, param, value):
        dJointSetPlane2DAngleParam(self.jid, param, value)


# PRJoint
cdef class PRJoint(Joint):
    """Prismatic and Rotoide Joint.

    Constructor::

      PRJoint(world, jointgroup=None)
    """

    def __cinit__(self, World world not None, jointgroup=None):
        cdef JointGroup jg
        cdef dJointGroupID jgid

        jgid = NULL
        if jointgroup != None:
            jg = jointgroup
            jgid = jg.gid
        self.jid = dJointCreatePR(world.wid, jgid)

    def __init__(self, World world not None, jointgroup=None):
        self.world = world
        if jointgroup != None:
            jointgroup._addjoint(self)

    def getPosition(self):
        """getPosition()

        Get a PRJoint's linear extension.  (i.e. the prismatic's extension)
        """
        return dJointGetPRPosition(self.jid)

    def setAnchor(self, pos):
        """setAnchor(pos)

        Set a PRJoint anchor.

        @param pos: Anchor position
        @type pos: 3-sequence of floats
        """
        dJointSetPRAnchor(self.jid, pos[0], pos[1], pos[2])

    def getAnchor(self):
        """getAnchor()

        Get a PRJoint anchor.
        """
        cdef dVector3 a
        dJointGetPRAnchor(self.jid, a)
        return a[0], a[1], a[2]

    def setAxis1(self, axis):
        """setAxis1(axis)

        Set a PRJoint's prismatic axis.

        @param axis: Axis
        @type axis: 3-sequence of floats
        """
        dJointSetPRAxis1(self.jid, axis[0], axis[1], axis[2])

    def getAxis1(self):
        """getAxis1()

        Get a PRJoint's prismatic axis.
        """
        cdef dVector3 a
        dJointGetPRAxis1(self.jid, a)
        return a[0], a[1], a[2]

    def setAxis2(self, axis):
        """setAxis2(axis)

        Set a PRJoint's rotoide axis.

        @param axis: Axis
        @type axis: 3-sequence of floats
        """
        dJointSetPRAxis2(self.jid, axis[0], axis[1], axis[2])

    def getAxis2(self):
        """getAxis2()

        Get a PRJoint's rotoide axis.
        """
        cdef dVector3 a
        dJointGetPRAxis2(self.jid, a)
        return a[0], a[1], a[2]


# Geom base class
cdef class GeomObject:
    """This is the abstract base class for all geom objects.
    """
    
    # The id of the geom object as returned by dCreateXxxx()
    cdef dGeomID gid
    # The space in which the geom was placed (or None). This reference
    # is kept so that the space won't be destroyed while there are still
    # geoms around that might use it.
    cdef object space
    # The body that the geom was attached to (or None).
    cdef object body

    # A dictionary with user defined attributes
    cdef object attribs

    cdef object __weakref__

    def __cinit__(self, *a, **kw):
        self.gid = NULL
        self.space = None
        self.body = None
        self.attribs = {}

    def __init__(self, *a, **kw):
        raise NotImplementedError(
            "GeomObject base class can't be used directly")

    def __dealloc__(self):
        if self.gid != NULL:
            dGeomDestroy(self.gid)
            self.gid = NULL

    def __getattr__(self, name):
        if name in self.attribs:
            return self.attribs[name]
        else:
            raise AttributeError("geom has no attribute '%s'" % name)

    def __setattr__(self, name, val):
        self.attribs[name] = val

    def __delattr__(self, name):
        if name in self.attribs:
            del self.attribs[name]
        else:
            raise AttributeError("geom has no attribute '%s'" % name)

    def _id(self):
        """_id() -> int

        Return the internal id of the geom (dGeomID) as returned by
        the dCreateXyz() functions.

        This method has to be overwritten in derived methods.
        """
        raise NotImplementedError("Bug: The _id() method is not implemented")

    def placeable(self):
        """placeable() -> bool

        Returns True if the geom object is a placeable geom.

        This method has to be overwritten in derived methods.
        """
        return False

    def setBody(self, Body body):
        """setBody(body)

        Set the body associated with a placeable geom.

        @param body: The Body object or None.
        @type body: Body
        """

        if not self.placeable():
            raise ValueError(
                "Non-placeable geoms cannot have a body associated to them")
        
        if body == None:
            dGeomSetBody(self.gid, NULL)
        else:
            dGeomSetBody(self.gid, body.bid)
        self.body = body

    def getBody(self):
        """getBody() -> Body

        Get the body associated with this geom.
        """
        if not self.placeable():
            return environment
        
        return self.body

    def setPosition(self, pos):
        """setPosition(pos)

        Set the position of the geom. If the geom is attached to a body,
        the body's position will also be changed.

        @param pos: Position
        @type pos: 3-sequence of floats
        """
        if not self.placeable():
            raise ValueError("Cannot set a position on non-placeable geoms")
        dGeomSetPosition(self.gid, pos[0], pos[1], pos[2])

    def getPosition(self):
        """getPosition() -> 3-tuple

        Get the current position of the geom. If the geom is attached to
        a body the returned value is the body's position.
        """
        if not self.placeable():
            raise ValueError("Non-placeable geoms do not have a position")

        cdef dReal* p
        p = <dReal*>dGeomGetPosition(self.gid)
        return p[0], p[1], p[2]

    def setRotation(self, R):
        """setRotation(R)

        Set the orientation of the geom. If the geom is attached to a body,
        the body's orientation will also be changed.

        @param R: Rotation matrix
        @type R: 9-sequence of floats
        """
        if not self.placeable():
            raise ValueError("Cannot set a rotation on non-placeable geoms")

        cdef dMatrix3 m
        m[0] = R[0]
        m[1] = R[1]
        m[2] = R[2]
        m[3] = 0
        m[4] = R[3]
        m[5] = R[4]
        m[6] = R[5]
        m[7] = 0
        m[8] = R[6]
        m[9] = R[7]
        m[10] = R[8]
        m[11] = 0
        dGeomSetRotation(self.gid, m)

    def getRotation(self):
        """getRotation() -> 9-tuple

        Get the current orientation of the geom. If the geom is attached to
        a body the returned value is the body's orientation.
        """
        if not self.placeable():
            raise ValueError("Non-placeable geoms do not have a rotation")

        cdef dReal* m
        m = <dReal*>dGeomGetRotation(self.gid)
        return [m[0], m[1], m[2], m[4], m[5], m[6], m[8], m[9], m[10]]

    def getQuaternion(self):
        """getQuaternion() -> (w,x,y,z)

        Get the current orientation of the geom. If the geom is attached to
        a body the returned value is the body's orientation.
        """
        if not self.placeable():
            raise ValueError("Non-placeable geoms do not have an orientation")

        cdef dQuaternion q
        dGeomGetQuaternion(self.gid, q)
        return q[0], q[1], q[2], q[3]

    def setQuaternion(self, q):
        """setQuaternion(q)

        Set the orientation of the geom. If the geom is attached to a body,
        the body's orientation will also be changed.

        @param q: Quaternion (w,x,y,z)
        @type q: 4-sequence of floats
        """
        if not self.placeable():
            raise ValueError("Cannot set a quaternion on non-placeable geoms")

        cdef dQuaternion cq
        cq[0] = q[0]
        cq[1] = q[1]
        cq[2] = q[2]
        cq[3] = q[3]
        dGeomSetQuaternion(self.gid, cq)

    def setOffsetPosition(self, pos):
        """setOffsetPosition(pos)

        Set the offset position of the geom. The geom must be attached to a
        body.  If the geom did not have an offset, it is automatically created.
        This sets up an additional (local) transformation for the geom, since
        geoms attached to a body share their global position and rotation.

        @param pos: Position
        @type pos: 3-sequence of floats
        """
        if self.body == None:
            raise ValueError("Cannot set an offset position on a geom before "
                             "calling setBody")
        dGeomSetOffsetPosition(self.gid, pos[0], pos[1], pos[2])

    def getOffsetPosition(self):
        """getOffsetPosition() -> 3-tuple

        Get the offset position of the geom.
        """
        cdef dReal* p
        p = <dReal*>dGeomGetOffsetPosition(self.gid)
        return (p[0],p[1],p[2])

    def setOffsetRotation(self, R):
        """setOffsetRotation(R)

        Set the offset rotation of the geom. The geom must be attached to a
        body.  If the geom did not have an offset, it is automatically created.
        This sets up an additional (local) transformation for the geom, since
        geoms attached to a body share their global position and rotation.

        @param R: Rotation matrix
        @type R: 9-sequence of floats
        """
        if self.body == None:
            raise ValueError("Cannot set an offset rotation on a geom before "
                             "calling setBody")

        cdef dMatrix3 m
        m[0] = R[0]
        m[1] = R[1]
        m[2] = R[2]
        m[3] = 0
        m[4] = R[3]
        m[5] = R[4]
        m[6] = R[5]
        m[7] = 0
        m[8] = R[6]
        m[9] = R[7]
        m[10] = R[8]
        m[11] = 0
        dGeomSetOffsetRotation(self.gid, m)

    def getOffsetRotation(self):
        """getOffsetRotation() -> 9-tuple

        Get the offset rotation of the geom.
        """
        cdef dReal* m
        m = <dReal*>dGeomGetOffsetRotation(self.gid)
        return [m[0], m[1], m[2], m[4], m[5], m[6], m[8], m[9], m[10]]

    def clearOffset(self):
        """clearOffset()

        Disable the offset transform of the geom.
        """
        dGeomClearOffset(self.gid)

    def getAABB(self):
        """getAABB() -> 6-tuple

        Return an axis aligned bounding box that surrounds the geom.
        The return value is a 6-tuple (minx, maxx, miny, maxy, minz, maxz).
        """
        cdef dReal aabb[6]
        
        dGeomGetAABB(self.gid, aabb)
        return aabb[0], aabb[1], aabb[2], aabb[3], aabb[4], aabb[5]

    def isSpace(self):
        """isSpace() -> bool

        Return 1 if the given geom is a space, or 0 if not."""
        return dGeomIsSpace(self.gid)

    def getSpace(self):
        """getSpace() -> Space

        Return the space that the given geometry is contained in,
        or return None if it is not contained in any space."""
        return self.space

    def setCollideBits(self, bits):
        """setCollideBits(bits)

        Set the "collide" bitfields for this geom.

        @param bits: Collide bit field
        @type bits: int/long
        """
        dGeomSetCollideBits(self.gid, long(bits))
        
    def setCategoryBits(self, bits):
        """setCategoryBits(bits)

        Set the "category" bitfields for this geom.

        @param bits: Category bit field
        @type bits: int/long
        """
        dGeomSetCategoryBits(self.gid, long(bits))

    def getCollideBits(self):
        """getCollideBits() -> long

        Return the "collide" bitfields for this geom.
        """
        return dGeomGetCollideBits(self.gid)

    def getCategoryBits(self):
        """getCategoryBits() -> long

        Return the "category" bitfields for this geom.
        """
        return dGeomGetCategoryBits(self.gid)
    
    def enable(self):
        """enable()

        Enable the geom."""
        dGeomEnable(self.gid)

    def disable(self):
        """disable()

        Disable the geom."""
        dGeomDisable(self.gid)

    def isEnabled(self):
        """isEnabled() -> bool

        Return True if the geom is enabled."""
        return dGeomIsEnabled(self.gid)


# _SpaceIterator
class _SpaceIterator:
    """Iterates over the geoms inside a Space.
    """

    def __init__(self, space):
        self.space = space
        self.idx = 0
        
    def __iter__(self):
        return self

    def next(self):
        if self.idx >= self.space.getNumGeoms():
            raise StopIteration
        else:
            res = self.space.getGeom(self.idx)
            self.idx = self.idx + 1
            return res


# SpaceBase
cdef class SpaceBase(GeomObject):
    """Space class (container for geometry objects).

    A Space object is a container for geometry objects which are used
    to do collision detection.
    The space does high level collision culling, which means that it
    can identify which pairs of geometry objects are potentially
    touching.

    This Space class can be used for both, a SimpleSpace and a HashSpace
    (see ODE documentation).

     >>> space = Space(type=0)   # Create a SimpleSpace
     >>> space = Space(type=1)   # Create a HashSpace
    """

    # The id of the space. Actually this is a copy of the value in self.gid
    # (as the Space is derived from GeomObject) which can be used without
    # casting whenever a *space* id is required.
    cdef dSpaceID sid
    
    # Dictionary with Geomobjects. Key is the ID (geom._id()) and the value
    # is the geom object (Python wrapper). This is used in collide_callback()
#    cdef object geom_dict

    def __cinit__(self, *a, **kw):
        pass

    def __init__(self, *a, **kw):
        raise NotImplementedError("The SpaceBase class can't be used directly")

    def __dealloc__(self):
        if self.gid != NULL:
            dSpaceDestroy(self.sid)
            self.sid = NULL
            self.gid = NULL

#    def _addgeom(self, geom):
#        """Insert the geom object into the dictionary (internal method).
#
#        This method has to called in the constructor of a geom object.
#        """
#        self.geom_dict[geom._id()]=geom

#    def _id2geom(self, id):
#        """Get the Python wrapper that corresponds to an ID.
#
#        The ID is the integer value, as returned by geom._id().
#        If the ID is unknown then None is returned.
#        """
#        if id in self.geom_dict:
#            return self.geom_dict[id]
#        else:
#            return None
       
    def _id(self):
        cdef size_t id
        id = <size_t>self.sid
        return id

    def __len__(self):
        return self.getNumGeoms()

    def __iter__(self):
        return _SpaceIterator(self)

    def add(self, GeomObject geom):
        """add(geom)

        Add a geom to a space. This does nothing if the geom is
        already in the space.

        @param geom: Geom object to add
        @type geom: GeomObject
        """
        
        dSpaceAdd(self.sid, geom.gid)

    def remove(self, GeomObject geom):
        """remove(geom)

        Remove a geom from a space.

        @param geom: Geom object to remove
        @type geom: GeomObject
        """
        dSpaceRemove(self.sid, geom.gid)

    def query(self, GeomObject geom):
        """query(geom) -> bool

        Return True if the given geom is in the space.

        @param geom: Geom object to check
        @type geom: GeomObject
        """
        return dSpaceQuery(self.sid, geom.gid)

    def getNumGeoms(self):
        """getNumGeoms() -> int

        Return the number of geoms contained within the space.
        """
        return dSpaceGetNumGeoms(self.sid)

    def getGeom(self, int idx):
        """getGeom(idx) -> GeomObject

        Return the geom with the given index contained within the space.

        @param idx: Geom index (0,1,...,getNumGeoms()-1)
        @type idx: int
        """
        cdef dGeomID gid

        # Check the index
        if idx < 0 or idx >= dSpaceGetNumGeoms(self.sid):
            raise IndexError("geom index out of range")

        gid = dSpaceGetGeom(self.sid, idx)
        if <size_t>gid not in _geom_c2py_lut:
            raise RuntimeError(
                "geom id cannot be translated to a Python object")

        return _geom_c2py_lut[<size_t>gid]

    def collide(self, arg, callback):
        """collide(arg, callback)

        Call a callback function one or more times, for all
        potentially intersecting objects in the space. The callback
        function takes 3 arguments:

        def NearCallback(arg, geom1, geom2):

        The arg parameter is just passed on to the callback function.
        Its meaning is user defined. The geom1 and geom2 arguments are
        the geometry objects that may be near each other. The callback
        function can call the function collide() (not the Space
        method) on geom1 and geom2, perhaps first determining
        whether to collide them at all based on other information.

        @param arg: A user argument that is passed to the callback function
        @param callback: Callback function
        @type callback: callable
        """
        
        cdef void* data
        cdef object tup
        tup = (callback, arg)
        data = <void*>tup
        dSpaceCollide(self.sid, data, collide_callback)


# Callback function for the dSpaceCollide() call in the Space.collide() method
# The data parameter is a tuple (Python-Callback, Arguments).
# The function calls a Python callback function with 3 arguments:
# def callback(UserArg, Geom1, Geom2)
# Geom1 and Geom2 are instances of GeomXyz classes.
cdef void collide_callback(void* data, dGeomID o1, dGeomID o2):
    cdef object tup
#    cdef Space space
    cdef size_t id1, id2

#    if (dGeomGetBody(o1)==dGeomGetBody(o2)):
#        return
    
    tup = <object>data
    callback, arg = tup
    id1 = <size_t>o1
    id2 = <size_t>o2
    g1 = _geom_c2py_lut[id1]
    g2 = _geom_c2py_lut[id2]
    callback(arg, g1, g2)


# SimpleSpace
cdef class SimpleSpace(SpaceBase):
    """Simple space.

    This does not do any collision culling - it simply checks every
    possible pair of geoms for intersection, and reports the pairs
    whose AABBs overlap. The time required to do intersection testing
    for n objects is O(n**2). This should not be used for large numbers
    of objects, but it can be the preferred algorithm for a small
    number of objects. This is also useful for debugging potential
    problems with the collision system.
    """

    def __cinit__(self, space=None):
        cdef SpaceBase sp
        cdef dSpaceID parentid

        parentid = NULL
        if space != None:
            sp = space
            parentid = sp.sid
        
        self.sid = dSimpleSpaceCreate(parentid)

        # Copy the ID
        self.gid = <dGeomID>self.sid

        dSpaceSetCleanup(self.sid, 0)
        _geom_c2py_lut[<size_t>self.sid] = self

    def __init__(self, space=None):
        pass

# HashSpace
cdef class HashSpace(SpaceBase):
    """Multi-resolution hash table space.

    This uses an internal data structure that records how each geom
    overlaps cells in one of several three dimensional grids. Each
    grid has cubical cells of side lengths 2**i, where i is an integer
    that ranges from a minimum to a maximum value. The time required
    to do intersection testing for n objects is O(n) (as long as those
    objects are not clustered together too closely), as each object
    can be quickly paired with the objects around it.
    """

    def __cinit__(self, space=None):
        cdef SpaceBase sp
        cdef dSpaceID parentid

        parentid = NULL
        if space != None:
            sp = space
            parentid = sp.sid
        
        self.sid = dHashSpaceCreate(parentid)

        # Copy the ID
        self.gid = <dGeomID>self.sid

        dSpaceSetCleanup(self.sid, 0)
        _geom_c2py_lut[<size_t>self.sid] = self

    def __init__(self, space=None):
        pass
    
    def setLevels(self, int minlevel, int maxlevel):
        """setLevels(minlevel, maxlevel)

        Sets the size of the smallest and largest cell used in the
        hash table. The actual size will be 2^minlevel and 2^maxlevel
        respectively.
        """
        
        if minlevel > maxlevel:
            raise ValueError(
                "minlevel (%d) must be less than or equal to maxlevel (%d)" %
                (minlevel, maxlevel))
            
        dHashSpaceSetLevels(self.sid, minlevel, maxlevel)

    def getLevels(self):
        """getLevels() -> (minlevel, maxlevel)

        Gets the size of the smallest and largest cell used in the
        hash table. The actual size is 2^minlevel and 2^maxlevel
        respectively.
        """
        
        cdef int minlevel
        cdef int maxlevel
        dHashSpaceGetLevels(self.sid, &minlevel, &maxlevel)
        return minlevel, maxlevel


# QuadTreeSpace
cdef class QuadTreeSpace(SpaceBase):
    """Quadtree space.

    This uses a pre-allocated hierarchical grid-based AABB tree to
    quickly cull collision checks. It's exceptionally quick for large
    amounts of objects in landscape-shaped worlds. The amount of
    memory used is 4**depth * 32 bytes.

    Currently getGeom() is not implemented for the quadtree space.
    """

    def __cinit__(self, center, extents, depth, space=None):
        cdef SpaceBase sp
        cdef dSpaceID parentid
        cdef dVector3 c
        cdef dVector3 e

        parentid = NULL
        if space != None:
            sp = space
            parentid = sp.sid

        c[0] = center[0]
        c[1] = center[1]
        c[2] = center[2]
        e[0] = extents[0]
        e[1] = extents[1]
        e[2] = extents[2]
        self.sid = dQuadTreeSpaceCreate(parentid, c, e, depth)

        # Copy the ID
        self.gid = <dGeomID>self.sid

        dSpaceSetCleanup(self.sid, 0)
        _geom_c2py_lut[<size_t>self.sid] = self

    def __init__(self, center, extents, depth, space=None):
        pass


def Space(space_type=0):
    """Space factory function.

    Depending on the type argument this function either returns a
    SimpleSpace (space_type=0) or a HashSpace (space_type=1).

    This function is provided to remain compatible with previous
    versions of PyODE where there was only one Space class.
    
     >>> space = Space(space_type=0)   # Create a SimpleSpace
     >>> space = Space(space_type=1)   # Create a HashSpace
    """
    if space_type == 0:
        return SimpleSpace()
    elif space_type == 1:
        return HashSpace()
    else:
        raise ValueError("Unknown space type (%d)" % space_type)


# GeomSphere
cdef class GeomSphere(GeomObject):
    """Sphere geometry.

    This class represents a sphere centered at the origin.

    Constructor::
    
      GeomSphere(space=None, radius=1.0)
    """

    def __cinit__(self, space=None, radius=1.0):
        cdef SpaceBase sp
        cdef dSpaceID sid

        sid = NULL
        if space != None:
            sp = space
            sid = sp.sid
        self.gid = dCreateSphere(sid, radius)
#        if space != None:
#            space._addgeom(self)

        _geom_c2py_lut[<size_t>self.gid] = self

    def __init__(self, space=None, radius=1.0):
        self.space = space
        self.body = None

    def placeable(self):
        return True

    def _id(self):
        cdef size_t id
        id = <size_t>self.gid
        return id

    def setRadius(self, radius):
        """setRadius(radius)

        Set the radius of the sphere.

        @param radius: New radius
        @type radius: float
        """
        dGeomSphereSetRadius(self.gid, radius)

    def getRadius(self):
        """getRadius() -> float

        Return the radius of the sphere.
        """
        return dGeomSphereGetRadius(self.gid)

    def pointDepth(self, p):
        """pointDepth(p) -> float

        Return the depth of the point p in the sphere. Points inside
        the geom will have positive depth, points outside it will have
        negative depth, and points on the surface will have zero
        depth.

        @param p: Point
        @type p: 3-sequence of floats
        """
        return dGeomSpherePointDepth(self.gid, p[0], p[1], p[2])

                
# GeomBox
cdef class GeomBox(GeomObject):
    """Box geometry.

    This class represents a box centered at the origin.

    Constructor::
    
      GeomBox(space=None, lengths=(1.0, 1.0, 1.0))
    """

    def __cinit__(self, space=None, lengths=(1.0, 1.0, 1.0)):
        cdef SpaceBase sp
        cdef dSpaceID sid
        
        sid = NULL
        if space != None:
            sp = space
            sid = sp.sid
        self.gid = dCreateBox(sid, lengths[0], lengths[1], lengths[2])
#        if space != None:
#            space._addgeom(self)

        _geom_c2py_lut[<size_t>self.gid] = self

    def __init__(self, space=None, lengths=(1.0, 1.0, 1.0)):
        self.space = space
        self.body = None

    def placeable(self):
        return True

    def _id(self):
        cdef size_t id
        id = <size_t>self.gid
        return id

    def setLengths(self, lengths):
        dGeomBoxSetLengths(self.gid, lengths[0], lengths[1], lengths[2])

    def getLengths(self):
        cdef dVector3 res
        dGeomBoxGetLengths(self.gid, res)
        return res[0], res[1], res[2]

    def pointDepth(self, p):
        """pointDepth(p) -> float

        Return the depth of the point p in the box. Points inside the
        geom will have positive depth, points outside it will have
        negative depth, and points on the surface will have zero
        depth.

        @param p: Point
        @type p: 3-sequence of floats
        """
        return dGeomBoxPointDepth(self.gid, p[0], p[1], p[2])


# GeomPlane
cdef class GeomPlane(GeomObject):
    """Plane geometry.

    This class represents an infinite plane. The plane equation is:
    n.x*x + n.y*y + n.z*z = dist

    This object can't be attached to a body.
    If you call getBody() on this object it always returns ode.environment.

    Constructor::
    
      GeomPlane(space=None, normal=(0,0,1), dist=0)

    """

    def __cinit__(self, space=None, normal=(0, 0, 1), dist=0):
        cdef SpaceBase sp
        cdef dSpaceID sid
        
        sid = NULL
        if space != None:
            sp = space
            sid = sp.sid
        self.gid = dCreatePlane(sid, normal[0], normal[1], normal[2], dist)
#        if space != None:
#            space._addgeom(self)

        _geom_c2py_lut[<size_t>self.gid] = self

    def __init__(self, space=None, normal=(0, 0, 1), dist=0):
        self.space = space

    def _id(self):
        cdef size_t id
        id = <size_t>self.gid
        return id

    def setParams(self, normal, dist):
        dGeomPlaneSetParams(self.gid, normal[0], normal[1], normal[2], dist)

    def getParams(self):
        cdef dVector4 res
        dGeomPlaneGetParams(self.gid, res)
        return ((res[0], res[1], res[2]), res[3])

    def pointDepth(self, p):
        """pointDepth(p) -> float

        Return the depth of the point p in the plane. Points inside the
        geom will have positive depth, points outside it will have
        negative depth, and points on the surface will have zero
        depth.

        @param p: Point
        @type p: 3-sequence of floats
        """
        return dGeomPlanePointDepth(self.gid, p[0], p[1], p[2])


# GeomCapsule
cdef class GeomCapsule(GeomObject):
    """Capped cylinder geometry.

    This class represents a capped cylinder aligned along the local Z axis
    and centered at the origin.

    Constructor::
    
      GeomCapsule(space=None, radius=0.5, length=1.0)

    The length parameter does not include the caps.
    """

    def __cinit__(self, space=None, radius=0.5, length=1.0):
        cdef SpaceBase sp
        cdef dSpaceID sid
        
        sid = NULL
        if space != None:
            sp = space
            sid = sp.sid
        self.gid = dCreateCapsule(sid, radius, length)
#        if space != None:
#            space._addgeom(self)

        _geom_c2py_lut[<size_t>self.gid] = self

    def __init__(self, space=None, radius=0.5, length=1.0):
        self.space = space
        self.body = None

    def placeable(self):
        return True

    def _id(self):
        cdef size_t id
        id = <size_t>self.gid
        return id

    def setParams(self, radius, length):
        dGeomCapsuleSetParams(self.gid, radius, length)

    def getParams(self):
        cdef dReal radius, length
        dGeomCapsuleGetParams(self.gid, &radius, &length)
        return radius, length

    def pointDepth(self, p):
        """pointDepth(p) -> float

        Return the depth of the point p in the cylinder. Points inside the
        geom will have positive depth, points outside it will have
        negative depth, and points on the surface will have zero
        depth.

        @param p: Point
        @type p: 3-sequence of floats
        """
        return dGeomCapsulePointDepth(self.gid, p[0], p[1], p[2])

GeomCCylinder = GeomCapsule # backwards compatibility


# GeomCylinder
cdef class GeomCylinder(GeomObject):
    """Plain cylinder geometry.

    This class represents an uncapped cylinder aligned along the local Z axis
    and centered at the origin.

    Constructor::
    
      GeomCylinder(space=None, radius=0.5, length=1.0)
    """

    def __cinit__(self, space=None, radius=0.5, length=1.0):
        cdef SpaceBase sp
        cdef dSpaceID sid
        
        sid = NULL
        if space != None:
            sp = space
            sid = sp.sid
        self.gid = dCreateCylinder(sid, radius, length)
#        if space != None:
#            space._addgeom(self)

        _geom_c2py_lut[<size_t>self.gid] = self

    def __init__(self, space=None, radius=0.5, length=1.0):
        self.space = space
        self.body = None

    def placeable(self):
        return True

    def _id(self):
        cdef size_t id
        id = <size_t>self.gid
        return id

    def setParams(self, radius, length):
        dGeomCylinderSetParams(self.gid, radius, length)

    def getParams(self):
        cdef dReal radius, length
        dGeomCylinderGetParams(self.gid, &radius, &length)
        return radius, length

    ## dGeomCylinderPointDepth not implemented upstream in ODE 0.7


# GeomRay
cdef class GeomRay(GeomObject):
    """Ray object.

    A ray is different from all the other geom classes in that it does
    not represent a solid object. It is an infinitely thin line that
    starts from the geom's position and extends in the direction of
    the geom's local Z-axis.

    Constructor::
    
      GeomRay(space=None, rlen=1.0)
    
    """

    def __cinit__(self, space=None, rlen=1.0):
        cdef SpaceBase sp
        cdef dSpaceID sid
        
        sid = NULL
        if space != None:
            sp = space
            sid = sp.sid
        self.gid = dCreateRay(sid, rlen)
#        if space != None:
#            space._addgeom(self)

        _geom_c2py_lut[<size_t>self.gid] = self

    def __init__(self, space=None, rlen=1.0):
        self.space = space
        self.body = None

    def _id(self):
        cdef size_t id
        id = <size_t>self.gid
        return id

    def placeable(self):
        return True

    def setLength(self, rlen):
        '''setLength(rlen)

        Set length of the ray.

        @param rlen: length of the ray
        @type rlen: float'''
        dGeomRaySetLength(self.gid, rlen)

    def getLength(self):
        '''getLength() -> length

        Get the length of the ray.

        @returns: length of the ray (float)'''
        return dGeomRayGetLength(self.gid)

    def set(self, p, u):
        '''set(p, u)

        Set the position and rotation of a ray.
        
        @param p: position
        @type p: 3-sequence of floats
        @param u: rotation
        @type u: 3-sequence of floats'''
        dGeomRaySet(self.gid, p[0], p[1], p[2], u[0], u[1], u[2])

    def get(self):
        '''get() -> ((p[0], p[1], p[2]), (u[0], u[1], u[2]))

        Return the position and rotation as a pair of
        tuples.

        @returns: position and rotation'''
        cdef dVector3 start
        cdef dVector3 dir
        dGeomRayGet(self.gid, start, dir)
        return (start[0], start[1], start[2]), (dir[0], dir[1], dir[2])


# GeomTransform
cdef class GeomTransform(GeomObject):
    """GeomTransform.

    A geometry transform "T" is a geom that encapsulates another geom
    "E", allowing E to be positioned and rotated arbitrarily with
    respect to its point of reference.

    Constructor::
    
      GeomTransform(space=None)
    """

    cdef object geom

    def __cinit__(self, space=None):
        cdef SpaceBase sp
        cdef dSpaceID sid
        
        sid = NULL
        if space != None:
            sp = space
            sid = sp.sid
        self.gid = dCreateGeomTransform(sid)
        # Set cleanup mode to 0 as a contained geom will be deleted
        # by its Python wrapper class
        dGeomTransformSetCleanup(self.gid, 0)
#        if space != None:
#            space._addgeom(self)

        _geom_c2py_lut[<size_t>self.gid] = self

    def __init__(self, space=None):
        self.space = space
        self.body = None
        self.geom = None

        self.attribs = {}

    def placeable(self):
        return True

    def _id(self):
        cdef size_t id
        id = <size_t>self.gid
        return id

    def setGeom(self, GeomObject geom not None):
        """setGeom(geom)

        Set the geom that the geometry transform encapsulates.
        A ValueError exception is thrown if a) the geom is not placeable,
        b) the geom was already inserted into a space or c) the geom is
        already associated with a body.

        @param geom: Geom object to encapsulate
        @type geom: GeomObject
        """
        cdef size_t id

        if not geom.placeable():
            raise ValueError(
                "Only placeable geoms can be encapsulated by a GeomTransform")
        if dGeomGetSpace(geom.gid) != <dSpaceID>0:
            raise ValueError(
                "The encapsulated geom was already inserted into a space")
        if dGeomGetBody(geom.gid) != <dBodyID>0:
            raise ValueError(
                "The encapsulated geom is already associated with a body")
        
        id = geom._id()
        dGeomTransformSetGeom(self.gid, <dGeomID>id)
        self.geom = geom

    def getGeom(self):
        """getGeom() -> GeomObject

        Get the geom that the geometry transform encapsulates.
        """
        return self.geom

    def setInfo(self, int mode):
        """setInfo(mode)

        Set the "information" mode of the geometry transform.

        With mode 0, when a transform object is collided with another
        object, the geom field of the ContactGeom structure is set to the
        geom that is encapsulated by the transform object.

        With mode 1, the geom field of the ContactGeom structure is set
        to the transform object itself.

        @param mode: Information mode (0 or 1)
        @type mode: int
        """
        if mode < 0 or mode > 1:
            raise ValueError(
                "Invalid information mode (%d). Must be either 0 or 1." % mode)
        dGeomTransformSetInfo(self.gid, mode)

    def getInfo(self):
        """getInfo() -> int

        Get the "information" mode of the geometry transform (0 or 1).

        With mode 0, when a transform object is collided with another
        object, the geom field of the ContactGeom structure is set to the
        geom that is encapsulated by the transform object.

        With mode 1, the geom field of the ContactGeom structure is set
        to the transform object itself.
        """
        return dGeomTransformGetInfo(self.gid)

######################################################################


cdef class TriMeshData:
    """This class stores the mesh data.
    """

    cdef dTriMeshDataID tmdid
    cdef dReal* vertex_buffer
    cdef int* face_buffer

    def __cinit__(self):
        self.tmdid = dGeomTriMeshDataCreate()
        self.vertex_buffer = NULL
        self.face_buffer = NULL

    def __dealloc__(self):
        if self.tmdid != NULL:
            dGeomTriMeshDataDestroy(self.tmdid)
        if self.vertex_buffer != NULL:
            free(self.vertex_buffer)
        if self.face_buffer != NULL:
            free(self.face_buffer)
    
    def build(self, verts, faces):
        """build(verts, faces)

        @param verts: Vertices
        @type verts: Sequence of 3-sequences of floats
        @param faces: Face definitions (three indices per face)
        @type faces: Sequence of 3-sequences of ints
        """
        cdef int numverts
        cdef int numfaces
        cdef dReal* vp
        cdef int* fp
        cdef int a, b, c
        
        numverts = len(verts)
        numfaces = len(faces)
        # Allocate the vertex and face buffer
        self.vertex_buffer = <dReal*>malloc(numverts * 4 * sizeof(dReal))
        self.face_buffer = <int*>malloc(numfaces * 3 * sizeof(int))

        # Fill the vertex buffer
        vp = self.vertex_buffer
        for v in verts:
            vp[0] = v[0]
            vp[1] = v[1]
            vp[2] = v[2]
            vp[3] = 0
            vp = vp + 4

        # Fill the face buffer
        fp = self.face_buffer
        for f in faces:
            a = f[0]
            b = f[1]
            c = f[2]
            if (a < 0 or b < 0 or c < 0 or a >= numverts or b >= numverts or c >= numverts):
                raise ValueError("Vertex index out of range")
            fp[0] = a
            fp[1] = b
            fp[2] = c
            fp = fp + 3

        # Pass the data to ODE
        dGeomTriMeshDataBuildSimple(self.tmdid, self.vertex_buffer, numverts,
                                    self.face_buffer, numfaces * 3)

######################################################################


# GeomTriMesh
cdef class GeomTriMesh(GeomObject):
    """TriMesh object.

    To construct the trimesh geom you need a TriMeshData object that
    stores the actual mesh. This object has to be passed as first
    argument to the constructor.

    Constructor::
    
      GeomTriMesh(data, space=None)
    """

    # Keep a reference to the data
    cdef TriMeshData data

    def __cinit__(self, TriMeshData data not None, space=None):
        cdef SpaceBase sp
        cdef dSpaceID sid

        self.data = data

        sid = NULL
        if space != None:
            sp = space
            sid = sp.sid
        self.gid = dCreateTriMesh(sid, data.tmdid, NULL, NULL, NULL)

        _geom_c2py_lut[<size_t>self.gid] = self

    def __init__(self, TriMeshData data not None, space=None):
        self.space = space
        self.body = None

    def placeable(self):
        return True

    def _id(self):
        cdef size_t id
        id = <size_t>self.gid
        return id

    def clearTCCache(self):
        """clearTCCache()

        Clears the internal temporal coherence caches.
        """
        dGeomTriMeshClearTCCache(self.gid)

    def getTriangle(self, int idx):
        """getTriangle(idx) -> (v0, v1, v2)

        @param idx: Triangle index
        @type idx: int
        """

        cdef dVector3 v0, v1, v2
        cdef dVector3* vp0
        cdef dVector3* vp1
        cdef dVector3* vp2

        vp0 = <dVector3*>v0
        vp1 = <dVector3*>v1
        vp2 = <dVector3*>v2

        dGeomTriMeshGetTriangle(self.gid, idx, vp0, vp1, vp2)
        return ((v0[0], v0[1], v0[2]),
                (v1[0], v1[1], v1[2]),
                (v2[0], v2[1], v2[2]))
        
    def getTriangleCount(self):
        """getTriangleCount() -> n

        Returns the number of triangles in the TriMesh."""

        return dGeomTriMeshGetTriangleCount(self.gid)

######################################################################


def collide(geom1, geom2):
    """collide(geom1, geom2) -> contacts

    Generate contact information for two objects.

    Given two geometry objects that potentially touch (geom1 and geom2),
    generate contact information for them. Internally, this just calls
    the correct class-specific collision functions for geom1 and geom2.

    [flags specifies how contacts should be generated if the objects
    touch. Currently the lower 16 bits of flags specifies the maximum
    number of contact points to generate. If this number is zero, this
    function just pretends that it is one - in other words you can not
    ask for zero contacts. All other bits in flags must be zero. In
    the future the other bits may be used to select other contact
    generation strategies.]

    If the objects touch, this returns a list of Contact objects,
    otherwise it returns an empty list.

    @param geom1: First Geom
    @type geom1: GeomObject
    @param geom2: Second Geom
    @type geom2: GeomObject
    @returns: Returns a list of Contact objects.
    """
    
    cdef dContactGeom c[150]
    cdef size_t id1
    cdef size_t id2
    cdef int i, n
    cdef Contact cont

    id1 = geom1._id()
    id2 = geom2._id()

    n = dCollide(<dGeomID>id1, <dGeomID>id2, 150, c, sizeof(dContactGeom))
    res = []
    i = 0
    while i < n:
        cont = Contact()
        cont._contact.geom = c[i]
        res.append(cont)
        i = i + 1

    return res


def collide2(geom1, geom2, arg, callback):
    """collide2(geom1, geom2, arg, callback)
    
    Calls the callback for all potentially intersecting pairs that contain
    one geom from geom1 and one geom from geom2.

    @param geom1: First Geom
    @type geom1: GeomObject
    @param geom2: Second Geom
    @type geom2: GeomObject
    @param arg: A user argument that is passed to the callback function
    @param callback: Callback function
    @type callback: callable
    """
    cdef void* data
    cdef object tup
    cdef size_t id1
    cdef size_t id2

    id1 = geom1._id()
    id2 = geom2._id()
    
    tup = (callback, arg)
    data = <void*>tup
    # collide_callback is defined in space.pyx
    dSpaceCollide2(<dGeomID>id1, <dGeomID>id2, data, collide_callback)


def areConnected(Body body1, Body body2):
    """areConnected(body1, body2) -> bool

    Return True if the two bodies are connected together by a joint,
    otherwise return False.

    @param body1: First body
    @type body1: Body
    @param body2: Second body
    @type body2: Body
    @returns: True if the bodies are connected
    """

    if body1 is environment:
        return False
    if body2 is environment:
        return False

    return bool(dAreConnected(<dBodyID> body1.bid, <dBodyID> body2.bid))


def CloseODE():
    """CloseODE()

    Deallocate some extra memory used by ODE that can not be deallocated
    using the normal destroy functions.
    """
    dCloseODE()


def InitODE():
    '''InitODE()

    Initialize some ODE internals. This will be called for you when you
    "import ode", but you should call this again if you CloseODE().'''
    dInitODE()


#environment = Body(None)
environment = None
InitODE()