root / trunk / pysoy / src / bodies / Body.pym

Revision 1356, 14.7 kB (checked in by ArcRiley, 4 weeks ago)

Ticket #962 :

  • eliminated soy.bodies.Body._calcFogCoords
  • fixed some obvious problems with liquid_example.py
  • still renders just black
  • Property svn:keywords set to Id
Line 
1# PySoy's bodies.Body class
2#
3# Copyright (C) 2006,2007,2008 PySoy Group
4#
5#  This program is free software; you can redistribute it and/or modify
6#  it under the terms of the GNU Affero General Public License as published
7#  by the Free Software Foundation, either version 3 of the License, or
8#  (at your option) any later version.
9#
10#  This program is distributed in the hope that it will be useful,
11#  but WITHOUT ANY WARRANTY; without even the implied warranty of
12#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13#  GNU Affero General Public License for more details.
14#
15#  You should have received a copy of the GNU Affero General Public License
16#  along with this program; if not, see http://www.gnu.org/licenses
17#
18# $Id$
19 
20cdef class Body :
21  '''soy.bodies.Body
22
23    Bodies are generic objects in 3d space with a position, velocity,
24    mass.  Joints an shapes can be applied to them to govern movement.
25  '''
26
27  ##########################################################################
28  #
29  # Python Functions
30  #
31
32  def __cinit__(self, scene=None,
33                position=None, rotation=None, velocity=None,
34                model=None, shape=None,
35                *args, **keywords) :
36    #
37    # Preset to avoid threading bugs
38    self._model = None
39    self._shape = None
40    self._tags  = soy._datatypes.HashTable()
41    #
42    if scene == None :
43      self._copySet(<ode.dReal*> py.PyMem_Malloc(sizeof(float) * 25))
44      self._bodyID = NULL
45      self._scene  = None
46    elif isinstance(scene, soy.scenes.Scene) :
47      self._scene = scene
48      self._scene._stepLock()
49      self._create()
50      self._scene._bodies._append(<void*> self)
51      self._scene._stepUnLock()     
52    else :
53      self._scene    = None
54      self._position = NULL
55      raise TypeError('scene must be instance of soy.scenes.Scene or None')
56    #
57    # Now that these internals are available, set them:
58    if position :
59      self.position = position
60    if rotation :
61      self.rotation = rotation
62    if velocity :
63      self.velocity = velocity
64    self.model = model
65    self.shape = shape
66
67
68  def __dealloc__(self) :
69    if self._scene is not None :
70      self._scene._stepLock()
71      self._remove()
72      self._destroy()
73      self._scene._stepUnLock()
74    elif self._position != NULL :
75      # If not in a scene and local storage alloc'ed, free it
76      py.PyMem_Free(self._position)
77
78   
79  def distance(self, Body myBody) :
80    assert self._scene == myBody._scene, 'bodies are in different scenes'
81    return ode.dDISTANCE(self._position, myBody._position)
82
83   
84  def floor(self,Body floor) :
85    cdef ode.dGeomID _ray
86    cdef ode.dContactGeom cg[10]
87    cdef ode.dSpaceID _spaceID
88    _spaceID = self._scene._spaceID 
89    _ray = ode.dCreateRay(_spaceID,1000)
90    ode.dGeomRaySet(_ray,
91                    self._position[0], self._position[1], self._position[2],
92                    0, -1, 0)
93    countacts = ode.dCollide(<ode.dGeomID> _ray, <ode.dGeomID> _spaceID,
94                             10, cg, sizeof(cg[0]))
95    print "done"
96    for contact in range(countacts): # countacts contains amount of collisions
97      print "Trying a contact"
98      if ode.dGeomGetBody(cg[contact].g2) == floor._bodyID :
99        print "Floor found"
100        self.position = (self._position[0], cg[contact].pos[1],
101                         self._position[2])
102        break
103
104
105  ############################################################################
106  #
107  # Properties
108  #
109
110  property scene :
111    '''Body's Scene
112
113    This is the Scene that the body is in.  When changed the body's position,
114    orientation, velocity, and rotation are all reset to (0,0,0).
115    '''
116    def __get__(self) :
117      return self._scene
118    #
119    def __set__(self, soy.scenes.Scene _newscene) :
120      cdef ode.dReal   _store[25]
121      cdef ode.dGeomID _geomID
122      #
123      # Preset _geomID for speed
124      if self._shape is None :
125        _geomID = NULL
126      else :
127        _geomID = self._shape._geomID
128      if self._scene is not None :
129        self._scene._stepLock()
130        self._copyTo(_store)
131        self._remove()
132        self._destroy()
133        if self._shape is not None :
134          ode.dGeomSetBody(self._shape._geomID, NULL)
135          ode.dSpaceRemove(self._scene._spaceID, self._shape._geomID)
136        self._scene._stepUnLock()
137      else :
138        self._copyTo(_store)
139        py.PyMem_Free(self._position)
140      self._scene = _newscene
141      if self._scene is not None :
142        self._scene._stepLock()
143        self._create()
144        if self._shape is not None :
145          ode.dGeomSetBody(self._shape._geomID, self._bodyID)
146          ode.dSpaceAdd(self._scene._spaceID, self._shape._geomID)
147        self._copyFrom(_store)
148        self._append()
149        self._scene._stepUnLock()
150    #
151    def __del__(self) :
152      cdef ode.dReal* _store
153      if self._scene is None :
154        return
155      _store = <ode.dReal*> py.PyMem_Malloc(sizeof(float) * 25)
156      self._scene._stepLock()
157      self._copyTo(_store)
158      self._remove()
159      self._destroy()
160      self._copySet(_store)
161      self._scene._stepUnLock()
162
163
164  property tags :
165    '''Body's tags
166
167    This is the body's tagset.
168    '''
169    def __get__(self) :
170      return self._tags
171
172
173  property mass :
174    '''Body's mass
175   
176    This is the mass of the body in the scene.  Effects physics.
177    '''
178
179    def __get__(self) :
180      return self._mass.mass
181    #
182    def __set__(self, value) :
183      if type(value)!=float and type(value)!=int :
184        raise TypeError('Must provide an integer or float')
185      if value < 0 :
186        raise TypeError('No negative masses')
187      elif self._bodyID :
188        self._mass.mass = value
189        if value != 0 :
190          ode.dBodySetMass(self._bodyID, &self._mass)
191          ode.dBodySetGravityMode(self._bodyID, 1)
192        else:
193          ode.dBodySetGravityMode(self._bodyID, 0)
194      else :
195        self._mass.mass = value
196
197       
198  property position :
199    '''Body's Position
200
201    This is the (x,y,z) of where your body is in the scene.
202    Defaults to (0.0, 0.0, 0.0).
203    '''
204    def __get__(self) :
205      return soy._datatypes.BodyPosition(self)
206    def __set__(self, value) :
207      if type(value) != tuple and type(value) != list \
208         and not isinstance(value, soy._datatypes.BodyPosition) :
209        raise TypeError('Must provide a tuple, list, or soy._datatype.BodyPosition')
210      if len(value)!=3 :
211        raise TypeError('Must provide (x,y,z)')
212      if self._scene is not None :
213        self._scene._stepLock()
214        ode.dBodySetPosition(self._bodyID, value[0], value[1], value[2])
215        self._scene._stepUnLock()
216      else :
217        self._position[0] = value[0]
218        self._position[1] = value[1]
219        self._position[2] = value[2]
220
221
222  property rotation :
223    '''Body's Rotational Velocity
224
225    This is how fast your body is rotating on it's own axis.
226    Defaults to (0.0, 0.0, 0.0).
227    '''
228    def __get__(self) :
229      return (self._angularVel[0], self._angularVel[1], self._angularVel[2])
230    def __set__(self, value) :
231      if type(value)!=tuple and type(value)!=list :
232        raise TypeError('Must provide a tuple or list')
233      if len(value)!=3 :
234        raise TypeError('Must provide (x,y,z)')
235      if self._scene is not None :
236        self._scene._stepLock()
237        ode.dBodySetAngularVel(self._bodyID, value[0], value[1], value[2])
238        self._scene._stepUnLock()
239      else :
240        self._angularVel[0] = value[0]
241        self._angularVel[1] = value[1]
242        self._angularVel[2] = value[2]
243
244
245  property velocity :
246    '''Body's Linear Velocity
247
248    This is how fast and in which direction a body is moving in a scene.
249    Defaults to (0.0, 0.0, 0.0).
250    '''
251    def __get__(self) :
252      return (self._linearVel[0], self._linearVel[1], self._linearVel[2])
253    def __set__(self, value) :
254      if type(value)!=tuple and type(value)!=list :
255        raise TypeError('Must provide a tuple or list')
256      if len(value)!=3 :
257        raise TypeError('Must provide (x,y,z)')
258      if self._scene is not None :
259        self._scene._stepLock()
260        ode.dBodySetLinearVel(self._bodyID, value[0], value[1], value[2])
261        self._scene._stepUnLock()
262      else :
263        self._linearVel[0] = value[0]
264        self._linearVel[1] = value[1]
265        self._linearVel[2] = value[2]
266
267
268  property model :
269    '''Body's model
270
271    If an instance of soy.models.Model it will be rendered as this Body.
272    Defaults to None.
273    '''
274    def __get__(self) :
275      return self._model
276
277    def __set__(self, _model) :
278      if not (_model is None or isinstance(_model, soy.models.Model)) :
279        raise TypeError('must be an instance of soy.models.Model')
280      # renderlock
281      self._model = _model
282      # renderunlock
283
284
285  property shape :
286    '''Body's shape, if it has one.
287   
288    The shape used for collisions.
289    Defaults to None.
290    '''
291    def __get__(self) :
292      return self._shape
293
294    def __set__(self, value) :
295      cdef ode.dBodyID _bodyID
296      #
297      ####################################
298      #
299      # Standard Typecheck
300      #
301      if not isinstance(value, soy.shapes.Shape) and value is not None :
302        raise TypeError('Requires a Shape or None')
303      #
304      ####################################
305      #
306      # if in a scene, lock it and remove any current shape
307      #
308      if self._scene is not None :
309        self._scene._stepLock()
310        if self._shape is not None :
311          ode.dSpaceRemove(self._scene._spaceID, self._shape._geomID)
312          ode.dGeomSetBody(self._shape._geomID, NULL)
313      #
314      ####################################
315      #
316      # Assign shape and, if it's an actual shape, detach/attach it
317      #
318      self._shape = value
319      if self._shape is not None :
320        #
321        ##################################
322        #
323        # if the new shape is already attached, detach it
324        #
325        _bodyID = <ode.dBodyID> ode.dGeomGetBody(self._shape._geomID)
326        if _bodyID != NULL :
327          (<Body> ode.dBodyGetData(_bodyID)).shape = None
328        #
329        ##################################
330        #
331        # if we're in a scene, add geom to it's space
332        #
333        if self._scene is not None :
334          ode.dSpaceAdd(self._scene._spaceID, self._shape._geomID)
335          ode.dGeomSetBody(self._shape._geomID, self._bodyID)
336        #     
337        ##################################
338      #
339      ####################################
340      #
341      # if we're in a scene, unlock before we return
342      #
343      if self._scene is not None :
344        self._scene._stepUnLock()
345      #
346      ####################################
347
348
349  ##########################################################################
350  #
351  # C Functions
352  #
353
354  cdef void _create(self) :
355    #
356    # This function allows custom _create functions to be used by the classes
357    # which inherit Body.
358    #
359    ######################################
360    #
361    # Create a new ODE body and put a pointer to self in it's data storage
362    # The data pointer to self allows Scene stepping functions (ie, collision)
363    # to quickly access the Python object associated with a given Body.
364    #   
365    self._bodyID = ode.dBodyCreate(self._scene._worldID)
366    ode.dBodySetData(self._bodyID, <void*> self)
367    #
368    ######################################
369    #
370    # get local pointers to ODE body states so we can access them directly
371    #
372    ode.dBodyGetMass(self._bodyID, &self._mass)
373    self._position   = <ode.dReal*> ode.dBodyGetPosition  (self._bodyID)
374    self._rotation   = <ode.dReal*> ode.dBodyGetRotation  (self._bodyID)
375    self._quaternion = <ode.dReal*> ode.dBodyGetQuaternion(self._bodyID)
376    self._linearVel  = <ode.dReal*> ode.dBodyGetLinearVel (self._bodyID)
377    self._angularVel = <ode.dReal*> ode.dBodyGetAngularVel(self._bodyID)
378    #
379    ######################################
380
381
382  cdef void _destroy(self) :
383    ode.dBodyDestroy(self._bodyID)
384    self._bodyID = NULL
385
386
387  cdef void _append(self) :
388    #
389    # This function allows classes which inherit Body to use a different
390    # _append sequence, ie, Light also appends itself to self._scene._lights
391    #
392    self._scene._bodies._append(<void*> self)
393    #
394    ######################################
395
396
397  cdef void _remove(self) :
398    #
399    # This function allows classes which inherit Body to use a different
400    # _remove sequence, ie, Light also remove itself to self._scene._lights
401    #
402    self._scene._bodies._remove(<void*> self)
403    #
404    ######################################
405
406
407  cdef void _copyTo(self, ode.dReal* _store) :
408    cdef int _i
409    for _i from 0 <= _i < 3 :
410      _store[_i] = self._position[_i]
411    for _i from 0 <= _i < 12 :
412      _store[_i+3] = self._rotation[_i]
413    for _i from 0 <= _i < 4 :
414      _store[_i+15] = self._quaternion[_i]
415    for _i from 0 <= _i < 3 :
416      _store[_i+19] = self._linearVel[_i]
417    for _i from 0 <= _i < 3 :
418      _store[_i+22] = self._angularVel[_i]
419
420
421  cdef void _copySet(self, ode.dReal* _store) :
422    #
423    # This function mallocs local storage for body attributes when it's
424    # not in a scene.  The storage is as follows:
425    #   position*3, rotation*12, quaternion*4, linearvel*3, angularvel*3 = 25
426    #
427    self._position   = _store
428    self._rotation   = self._position   + 3
429    self._quaternion = self._rotation   + 12
430    self._linearVel  = self._quaternion + 4
431    self._angularVel = self._linearVel  + 3
432
433
434  cdef void _copyFrom(self, ode.dReal* _store) :
435    ode.dBodySetPosition  (self._bodyID,  _store[0],  _store[1],  _store[2])
436    ode.dBodySetRotation  (self._bodyID, &_store[3] )
437    ode.dBodySetQuaternion(self._bodyID, &_store[15])
438    ode.dBodySetLinearVel (self._bodyID,  _store[19], _store[20], _store[21])
439    ode.dBodySetAngularVel(self._bodyID,  _store[22], _store[23], _store[24])
440
441
442  cdef void _addForce(self, ode.dVector3 _vector) :
443    if self._scene is None :
444      raise UnboundLocalError('Body is not in a scene')
445    self._scene._stepLock()
446    ode.dBodyAddForce(self._bodyID, _vector[0], _vector[1], _vector[2])
447    self._scene._stepUnLock()
448
449
450  cdef void _addTorque(self, ode.dVector3 _vector) :
451    if self._scene is None :
452      raise UnboundLocalError('Body is not in a scene')
453    self._scene._stepLock()
454    ode.dBodyAddTorque(self._bodyID, _vector[0], _vector[1], _vector[2])
455    self._scene._stepUnLock()
456
457
458
459  ##########################################################################
460  #
461  # WindowLoop Functions
462  #
463
464  cdef void _getModelviewInv(self, float* _mtx) nogil :
465    _mtx[0]  = self._rotation[0]
466    _mtx[4]  = self._rotation[4]
467    _mtx[8]  = self._rotation[8]
468    _mtx[3]  = 0.0
469    _mtx[1]  = self._rotation[1]
470    _mtx[5]  = self._rotation[5]
471    _mtx[9]  = self._rotation[9]
472    _mtx[7]  = 0.0
473    _mtx[2]  = self._rotation[2]
474    _mtx[6]  = self._rotation[6]
475    _mtx[10] = self._rotation[10]
476    _mtx[11] = 0.0
477    _mtx[12] = -self._position[0]
478    _mtx[13] = -self._position[1]
479    _mtx[14] = -self._position[2]
480    _mtx[15] = 1.0
Note: See TracBrowser for help on using the browser.