# PySoy's bodies.Body class # # Copyright (C) 2006,2007,2008 PySoy Group # # This program is free software; you can redistribute it and/or modify # it under the terms of the GNU Affero General Public License as published # by the Free Software Foundation, either version 3 of the License, or # (at your option) any later version. # # This program 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 # GNU Affero General Public License for more details. # # You should have received a copy of the GNU Affero General Public License # along with this program; if not, see http://www.gnu.org/licenses # # $Id$ cdef class Body : '''soy.bodies.Body Bodies are generic objects in 3d space with a position, velocity, mass. Joints an shapes can be applied to them to govern movement. ''' ########################################################################## # # Python Functions # def __cinit__(self, scene=None, position=None, rotation=None, velocity=None, model=None, shape=None, *args, **keywords) : # # Preset to avoid threading bugs self._model = None self._shape = None self._tags = soy._datatypes.HashTable() # if scene == None : self._copySet( py.PyMem_Malloc(sizeof(float) * 25)) self._bodyID = NULL self._scene = None elif isinstance(scene, soy.scenes.Scene) : self._scene = scene self._scene._stepLock() self._create() self._scene._bodies._append( self) self._scene._stepUnLock() else : self._scene = None self._position = NULL raise TypeError('scene must be instance of soy.scenes.Scene or None') # # Now that these internals are available, set them: if position : self.position = position if rotation : self.rotation = rotation if velocity : self.velocity = velocity self.model = model self.shape = shape def __dealloc__(self) : if self._scene is not None : self._scene._stepLock() self._remove() self._destroy() self._scene._stepUnLock() elif self._position != NULL : # If not in a scene and local storage alloc'ed, free it py.PyMem_Free(self._position) def distance(self, Body myBody) : assert self._scene == myBody._scene, 'bodies are in different scenes' return ode.dDISTANCE(self._position, myBody._position) def floor(self,Body floor) : cdef ode.dGeomID _ray cdef ode.dContactGeom cg[10] cdef ode.dSpaceID _spaceID _spaceID = self._scene._spaceID _ray = ode.dCreateRay(_spaceID,1000) ode.dGeomRaySet(_ray, self._position[0], self._position[1], self._position[2], 0, -1, 0) countacts = ode.dCollide( _ray, _spaceID, 10, cg, sizeof(cg[0])) print "done" for contact in range(countacts): # countacts contains amount of collisions print "Trying a contact" if ode.dGeomGetBody(cg[contact].g2) == floor._bodyID : print "Floor found" self.position = (self._position[0], cg[contact].pos[1], self._position[2]) break ############################################################################ # # Properties # property scene : '''Body's Scene This is the Scene that the body is in. When changed the body's position, orientation, velocity, and rotation are all reset to (0,0,0). ''' def __get__(self) : return self._scene # def __set__(self, soy.scenes.Scene _newscene) : cdef ode.dReal _store[25] cdef ode.dGeomID _geomID # # Preset _geomID for speed if self._shape is None : _geomID = NULL else : _geomID = self._shape._geomID if self._scene is not None : self._scene._stepLock() self._copyTo(_store) self._remove() self._destroy() if self._shape is not None : ode.dGeomSetBody(self._shape._geomID, NULL) ode.dSpaceRemove(self._scene._spaceID, self._shape._geomID) self._scene._stepUnLock() else : self._copyTo(_store) py.PyMem_Free(self._position) self._scene = _newscene if self._scene is not None : self._scene._stepLock() self._create() if self._shape is not None : ode.dGeomSetBody(self._shape._geomID, self._bodyID) ode.dSpaceAdd(self._scene._spaceID, self._shape._geomID) self._copyFrom(_store) self._append() self._scene._stepUnLock() # def __del__(self) : cdef ode.dReal* _store if self._scene is None : return _store = py.PyMem_Malloc(sizeof(float) * 25) self._scene._stepLock() self._copyTo(_store) self._remove() self._destroy() self._copySet(_store) self._scene._stepUnLock() property tags : '''Body's tags This is the body's tagset. ''' def __get__(self) : return self._tags property mass : '''Body's mass This is the mass of the body in the scene. Effects physics. ''' def __get__(self) : return self._mass.mass # def __set__(self, value) : if type(value)!=float and type(value)!=int : raise TypeError('Must provide an integer or float') if value < 0 : raise TypeError('No negative masses') elif self._bodyID : self._mass.mass = value if value != 0 : ode.dBodySetMass(self._bodyID, &self._mass) ode.dBodySetGravityMode(self._bodyID, 1) else: ode.dBodySetGravityMode(self._bodyID, 0) else : self._mass.mass = value property position : '''Body's Position This is the (x,y,z) of where your body is in the scene. Defaults to (0.0, 0.0, 0.0). ''' def __get__(self) : return soy._datatypes.BodyPosition(self) def __set__(self, value) : if type(value) != tuple and type(value) != list \ and not isinstance(value, soy._datatypes.BodyPosition) : raise TypeError('Must provide a tuple, list, or soy._datatype.BodyPosition') if len(value)!=3 : raise TypeError('Must provide (x,y,z)') if self._scene is not None : self._scene._stepLock() ode.dBodySetPosition(self._bodyID, value[0], value[1], value[2]) self._scene._stepUnLock() else : self._position[0] = value[0] self._position[1] = value[1] self._position[2] = value[2] property rotation : '''Body's Rotational Velocity This is how fast your body is rotating on it's own axis. Defaults to (0.0, 0.0, 0.0). ''' def __get__(self) : return (self._angularVel[0], self._angularVel[1], self._angularVel[2]) def __set__(self, value) : if type(value)!=tuple and type(value)!=list : raise TypeError('Must provide a tuple or list') if len(value)!=3 : raise TypeError('Must provide (x,y,z)') if self._scene is not None : self._scene._stepLock() ode.dBodySetAngularVel(self._bodyID, value[0], value[1], value[2]) self._scene._stepUnLock() else : self._angularVel[0] = value[0] self._angularVel[1] = value[1] self._angularVel[2] = value[2] property velocity : '''Body's Linear Velocity This is how fast and in which direction a body is moving in a scene. Defaults to (0.0, 0.0, 0.0). ''' def __get__(self) : return (self._linearVel[0], self._linearVel[1], self._linearVel[2]) def __set__(self, value) : if type(value)!=tuple and type(value)!=list : raise TypeError('Must provide a tuple or list') if len(value)!=3 : raise TypeError('Must provide (x,y,z)') if self._scene is not None : self._scene._stepLock() ode.dBodySetLinearVel(self._bodyID, value[0], value[1], value[2]) self._scene._stepUnLock() else : self._linearVel[0] = value[0] self._linearVel[1] = value[1] self._linearVel[2] = value[2] property model : '''Body's model If an instance of soy.models.Model it will be rendered as this Body. Defaults to None. ''' def __get__(self) : return self._model def __set__(self, _model) : if not (_model is None or isinstance(_model, soy.models.Model)) : raise TypeError('must be an instance of soy.models.Model') # renderlock self._model = _model # renderunlock property shape : '''Body's shape, if it has one. The shape used for collisions. Defaults to None. ''' def __get__(self) : return self._shape def __set__(self, value) : cdef ode.dBodyID _bodyID # #################################### # # Standard Typecheck # if not isinstance(value, soy.shapes.Shape) and value is not None : raise TypeError('Requires a Shape or None') # #################################### # # if in a scene, lock it and remove any current shape # if self._scene is not None : self._scene._stepLock() if self._shape is not None : ode.dSpaceRemove(self._scene._spaceID, self._shape._geomID) ode.dGeomSetBody(self._shape._geomID, NULL) # #################################### # # Assign shape and, if it's an actual shape, detach/attach it # self._shape = value if self._shape is not None : # ################################## # # if the new shape is already attached, detach it # _bodyID = ode.dGeomGetBody(self._shape._geomID) if _bodyID != NULL : ( ode.dBodyGetData(_bodyID)).shape = None # ################################## # # if we're in a scene, add geom to it's space # if self._scene is not None : ode.dSpaceAdd(self._scene._spaceID, self._shape._geomID) ode.dGeomSetBody(self._shape._geomID, self._bodyID) # ################################## # #################################### # # if we're in a scene, unlock before we return # if self._scene is not None : self._scene._stepUnLock() # #################################### ########################################################################## # # C Functions # cdef void _create(self) : # # This function allows custom _create functions to be used by the classes # which inherit Body. # ###################################### # # Create a new ODE body and put a pointer to self in it's data storage # The data pointer to self allows Scene stepping functions (ie, collision) # to quickly access the Python object associated with a given Body. # self._bodyID = ode.dBodyCreate(self._scene._worldID) ode.dBodySetData(self._bodyID, self) # ###################################### # # get local pointers to ODE body states so we can access them directly # ode.dBodyGetMass(self._bodyID, &self._mass) self._position = ode.dBodyGetPosition (self._bodyID) self._rotation = ode.dBodyGetRotation (self._bodyID) self._quaternion = ode.dBodyGetQuaternion(self._bodyID) self._linearVel = ode.dBodyGetLinearVel (self._bodyID) self._angularVel = ode.dBodyGetAngularVel(self._bodyID) # ###################################### cdef void _destroy(self) : ode.dBodyDestroy(self._bodyID) self._bodyID = NULL cdef void _append(self) : # # This function allows classes which inherit Body to use a different # _append sequence, ie, Light also appends itself to self._scene._lights # self._scene._bodies._append( self) # ###################################### cdef void _remove(self) : # # This function allows classes which inherit Body to use a different # _remove sequence, ie, Light also remove itself to self._scene._lights # self._scene._bodies._remove( self) # ###################################### cdef void _copyTo(self, ode.dReal* _store) : cdef int _i for _i from 0 <= _i < 3 : _store[_i] = self._position[_i] for _i from 0 <= _i < 12 : _store[_i+3] = self._rotation[_i] for _i from 0 <= _i < 4 : _store[_i+15] = self._quaternion[_i] for _i from 0 <= _i < 3 : _store[_i+19] = self._linearVel[_i] for _i from 0 <= _i < 3 : _store[_i+22] = self._angularVel[_i] cdef void _copySet(self, ode.dReal* _store) : # # This function mallocs local storage for body attributes when it's # not in a scene. The storage is as follows: # position*3, rotation*12, quaternion*4, linearvel*3, angularvel*3 = 25 # self._position = _store self._rotation = self._position + 3 self._quaternion = self._rotation + 12 self._linearVel = self._quaternion + 4 self._angularVel = self._linearVel + 3 cdef void _copyFrom(self, ode.dReal* _store) : ode.dBodySetPosition (self._bodyID, _store[0], _store[1], _store[2]) ode.dBodySetRotation (self._bodyID, &_store[3] ) ode.dBodySetQuaternion(self._bodyID, &_store[15]) ode.dBodySetLinearVel (self._bodyID, _store[19], _store[20], _store[21]) ode.dBodySetAngularVel(self._bodyID, _store[22], _store[23], _store[24]) cdef void _addForce(self, ode.dVector3 _vector) : if self._scene is None : raise UnboundLocalError('Body is not in a scene') self._scene._stepLock() ode.dBodyAddForce(self._bodyID, _vector[0], _vector[1], _vector[2]) self._scene._stepUnLock() cdef void _addTorque(self, ode.dVector3 _vector) : if self._scene is None : raise UnboundLocalError('Body is not in a scene') self._scene._stepLock() ode.dBodyAddTorque(self._bodyID, _vector[0], _vector[1], _vector[2]) self._scene._stepUnLock() ########################################################################## # # WindowLoop Functions # cdef void _getModelviewInv(self, float* _mtx) nogil : _mtx[0] = self._rotation[0] _mtx[4] = self._rotation[4] _mtx[8] = self._rotation[8] _mtx[3] = 0.0 _mtx[1] = self._rotation[1] _mtx[5] = self._rotation[5] _mtx[9] = self._rotation[9] _mtx[7] = 0.0 _mtx[2] = self._rotation[2] _mtx[6] = self._rotation[6] _mtx[10] = self._rotation[10] _mtx[11] = 0.0 _mtx[12] = -self._position[0] _mtx[13] = -self._position[1] _mtx[14] = -self._position[2] _mtx[15] = 1.0