Speed up the physics engine: hook the SOLID broad phase, so we can either reject...
[blender.git] / source / gameengine / Ketsji / KX_RaySensor.cpp
1 /**
2  * Cast a ray and feel for objects
3  *
4  * $Id$
5  *
6  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
7  *
8  * This program is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * as published by the Free Software Foundation; either version 2
11  * of the License, or (at your option) any later version. The Blender
12  * Foundation also sells licenses for use in proprietary software under
13  * the Blender License.  See http://www.blender.org/BL/ for information
14  * about this.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
19  * GNU General Public License for more details.
20  *
21  * You should have received a copy of the GNU General Public License
22  * along with this program; if not, write to the Free Software Foundation,
23  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
24  *
25  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
26  * All rights reserved.
27  *
28  * The Original Code is: all of this file.
29  *
30  * Contributor(s): none yet.
31  *
32  * ***** END GPL/BL DUAL LICENSE BLOCK *****
33  */
34
35 #include "KX_RaySensor.h"
36 #include "SCA_EventManager.h"
37 #include "SCA_RandomEventManager.h"
38 #include "SCA_LogicManager.h"
39 #include "SCA_IObject.h"
40 #include "KX_ClientObjectInfo.h"
41 #include "KX_GameObject.h"
42 #include "KX_Scene.h"
43
44 #include "SumoPhysicsEnvironment.h"
45 #include "KX_SumoPhysicsController.h"
46
47 #ifdef HAVE_CONFIG_H
48 #include <config.h>
49 #endif
50
51 KX_RaySensor::KX_RaySensor(class SCA_EventManager* eventmgr,
52                                         SCA_IObject* gameobj,
53                                         const STR_String& propname,
54                                         bool bFindMaterial,
55                                         double distance,
56                                         int axis,
57                                         KX_Scene* ketsjiScene,
58                                         PyTypeObject* T)
59                         : SCA_ISensor(gameobj,eventmgr, T),
60                                         m_propertyname(propname),
61                                         m_bFindMaterial(bFindMaterial),
62                                         m_distance(distance),
63                                         m_scene(ketsjiScene),
64                                         m_bTriggered(false),
65                                         m_axis(axis),
66                                         m_rayHit(false),
67                                         m_hitObject(NULL)
68
69                                 
70 {
71
72 }
73
74
75
76 KX_RaySensor::~KX_RaySensor() 
77 {
78     /* Nothing to be done here. */
79 }
80
81
82
83 CValue* KX_RaySensor::GetReplica()
84 {
85         CValue* replica = new KX_RaySensor(*this);
86         // this will copy properties and so on...
87         CValue::AddDataToReplica(replica);
88
89         return replica;
90 }
91
92
93
94 bool KX_RaySensor::IsPositiveTrigger()
95 {
96         bool result = m_rayHit;
97
98         if (m_invert)
99                 result = !result;
100         
101         return result;
102 }
103
104
105
106 bool KX_RaySensor::Evaluate(CValue* event)
107 {
108         bool result = false;
109         m_rayHit = false; 
110         m_hitObject = NULL;
111         m_hitPosition = MT_Vector3(0,0,0);
112         m_hitNormal = MT_Vector3(1,0,0);
113         
114         KX_GameObject* obj = (KX_GameObject*)GetParent();
115         MT_Point3 frompoint = obj->NodeGetWorldPosition();
116         MT_Matrix3x3 matje = obj->NodeGetWorldOrientation();
117         MT_Matrix3x3 invmat = matje.inverse();
118         
119         MT_Vector3 todir;
120         switch (m_axis)
121         {
122         case 1: // X
123                 {
124                         todir[0] = invmat[0][0];
125                         todir[1] = invmat[0][1];
126                         todir[2] = invmat[0][2];
127                         break;
128                 }
129         case 0: // Y
130                 {
131                         todir[0] = invmat[1][0];
132                         todir[1] = invmat[1][1];
133                         todir[2] = invmat[1][2];
134                         break;
135                 }
136         case 2: // Z
137                 {
138                         todir[0] = invmat[2][0];
139                         todir[1] = invmat[2][1];
140                         todir[2] = invmat[2][2];
141                         break;
142                 }
143         case 3: // -X
144                 {
145                         todir[0] = -invmat[0][0];
146                         todir[1] = -invmat[0][1];
147                         todir[2] = -invmat[0][2];
148                         break;
149                 }
150         case 4: // -Y
151                 {
152                         todir[0] = -invmat[1][0];
153                         todir[1] = -invmat[1][1];
154                         todir[2] = -invmat[1][2];
155                         break;
156                 }
157         case 5: // -Z
158                 {
159                         todir[0] = -invmat[2][0];
160                         todir[1] = -invmat[2][1];
161                         todir[2] = -invmat[2][2];
162                         break;
163                 }
164         }
165         todir.normalize();
166         m_rayDirection = todir;
167
168         MT_Point3 topoint = frompoint + (m_distance) * todir;
169         MT_Point3 resultpoint;
170         MT_Vector3 resultnormal;
171         bool ready = false;
172         SumoPhysicsEnvironment *spe = dynamic_cast<SumoPhysicsEnvironment *>(m_scene->GetPhysicsEnvironment());
173         if (!spe)
174         {
175                 std::cout << "WARNING: Ray sensor " << GetName() << ":  There is no physics environment!" << std::endl;
176                 std::cout << "         Check universe for malfunction." << std::endl;
177                 return false;
178         } 
179         SM_Scene *scene = spe->GetSumoScene();
180         KX_SumoPhysicsController *spc = dynamic_cast<KX_SumoPhysicsController *>(obj->GetPhysicsController());
181         KX_GameObject *parent = obj->GetParent();
182         if (!spc && parent)
183                 spc = dynamic_cast<KX_SumoPhysicsController *>(parent->GetPhysicsController());
184         if (parent)
185                 parent->Release();
186         SM_Object *thisObj = spc?spc->GetSumoObject():NULL;
187         
188         do {
189                 SM_Object* hitObj = scene->rayTest(thisObj,
190                         frompoint,
191                         topoint,
192                         resultpoint,
193                         resultnormal);
194                         
195                 if (hitObj)
196                 {
197         
198                         KX_ClientObjectInfo* info = static_cast<KX_ClientObjectInfo*>(hitObj->getClientObject());
199                         bool bFound = false;
200                         
201                         if (!info)
202                         {
203                                 std::cout<< "WARNING:  Ray sensor " << GetName() << " cannot sense SM_Object " << hitObj << " - no client info.\n" << std::endl;
204                                 ready = true;
205                                 break;
206                         } 
207                         
208                         SCA_IObject *hitgameobj = info->m_gameobject;
209                         
210                         if (hitgameobj == obj || info->m_type > KX_ClientObjectInfo::ACTOR)
211                         {
212                                 // false hit
213                                 KX_SumoPhysicsController *hitspc = dynamic_cast<KX_SumoPhysicsController *> (static_cast<KX_GameObject*> (hitgameobj) ->GetPhysicsController());
214                                 if (hitspc)
215                                 {
216                                         /* We add 0.01 of fudge, so that if the margin && radius == 0., we don't endless loop. */
217                                         MT_Scalar marg = 0.01 + hitspc->GetSumoObject()->getMargin();
218                                         if (hitspc->GetSumoObject()->getShapeProps())
219                                         {
220                                                 marg += 2*hitspc->GetSumoObject()->getShapeProps()->m_radius;
221                                         }
222                                         
223                                         /* Calculate the other side of this object */
224                                         MT_Point3 hitObjPos;
225                                         hitspc->GetWorldPosition(hitObjPos);
226                                         MT_Vector3 hitvector = hitObjPos - resultpoint;
227                                         if (hitvector.dot(hitvector) > MT_EPSILON)
228                                         {
229                                                 hitvector.normalize();
230                                                 marg *= 2.*todir.dot(hitvector);
231                                         }
232                                         frompoint = resultpoint + marg * todir;
233                                 } else {
234                                         ready = true;
235                                 }
236                         }
237                         else
238                         {
239                                 ready = true;
240                                 if (m_propertyname.Length() == 0)
241                                 {
242                                         bFound = true;
243                                 }
244                                 else
245                                 {
246                                         if (m_bFindMaterial)
247                                         {
248                                                 if (info->m_auxilary_info)
249                                                 {
250                                                         bFound = (m_propertyname== ((char*)info->m_auxilary_info));
251                                                 }
252                                         }
253                                         else
254                                         {
255                                                 bFound = hitgameobj->GetProperty(m_propertyname) != NULL;
256                                         }
257                                 }
258
259                                 if (bFound)
260                                 {
261                                         m_rayHit = true;
262                                         m_hitObject = hitgameobj;
263                                         m_hitPosition = resultpoint;
264                                         m_hitNormal = resultnormal;
265                                                 
266                                 }
267                         }
268                 }
269                 else
270                 {
271                         ready = true;
272                 }
273         }
274         while (!ready);
275         
276         
277         
278         /* now pass this result to some controller */
279     if (m_rayHit) 
280         {
281                 if (!m_bTriggered)
282                 {
283                         // notify logicsystem that ray is now hitting
284                         result = true;
285                         m_bTriggered = true;
286                 }
287                 else
288                 {
289                         
290                 }
291         }
292         else
293         {
294                 if (m_bTriggered)
295                 {
296                         m_bTriggered = false;
297                         // notify logicsystem that ray is not hitting anymore
298                         result = true;
299                 }
300         }
301
302         return result;
303 }
304
305
306
307 /* ------------------------------------------------------------------------- */
308 /* Python functions                                                          */
309 /* ------------------------------------------------------------------------- */
310
311 /* Integration hooks ------------------------------------------------------- */
312 PyTypeObject KX_RaySensor::Type = {
313         PyObject_HEAD_INIT(&PyType_Type)
314         0,
315         "KX_RaySensor",
316         sizeof(KX_RaySensor),
317         0,
318         PyDestructor,
319         0,
320         __getattr,
321         __setattr,
322         0, //&MyPyCompare,
323         __repr,
324         0, //&cvalue_as_number,
325         0,
326         0,
327         0,
328         0
329 };
330
331 PyParentObject KX_RaySensor::Parents[] = {
332         &KX_RaySensor::Type,
333         &SCA_ISensor::Type,
334         &SCA_ILogicBrick::Type,
335         &CValue::Type,
336         NULL
337 };
338
339 PyMethodDef KX_RaySensor::Methods[] = {
340         {"getHitObject",(PyCFunction) KX_RaySensor::sPyGetHitObject,METH_VARARGS, GetHitObject_doc},
341         {"getHitPosition",(PyCFunction) KX_RaySensor::sPyGetHitPosition,METH_VARARGS, GetHitPosition_doc},
342         {"getHitNormal",(PyCFunction) KX_RaySensor::sPyGetHitNormal,METH_VARARGS, GetHitNormal_doc},
343         {"getRayDirection",(PyCFunction) KX_RaySensor::sPyGetRayDirection,METH_VARARGS, GetRayDirection_doc},
344         {NULL,NULL} //Sentinel
345 };
346
347 char KX_RaySensor::GetHitObject_doc[] = 
348 "getHitObject()\n"
349 "\tReturns the name of the object that was hit by this ray.\n";
350 PyObject* KX_RaySensor::PyGetHitObject(PyObject* self, 
351                                                                                    PyObject* args, 
352                                                                                    PyObject* kwds)
353 {
354         if (m_hitObject)
355         {
356                 return m_hitObject->AddRef();
357         }
358         Py_Return;
359 }
360
361
362 char KX_RaySensor::GetHitPosition_doc[] = 
363 "getHitPosition()\n"
364 "\tReturns the position (in worldcoordinates) where the object was hit by this ray.\n";
365 PyObject* KX_RaySensor::PyGetHitPosition(PyObject* self, 
366                                PyObject* args, 
367                                PyObject* kwds)
368 {
369
370         MT_Point3 pos = m_hitPosition;
371
372         PyObject* resultlist = PyList_New(3);
373         int index;
374         for (index=0;index<3;index++)
375         {
376                 PyList_SetItem(resultlist,index,PyFloat_FromDouble(pos[index]));
377         }
378         return resultlist;
379
380 }
381
382 char KX_RaySensor::GetRayDirection_doc[] = 
383 "getRayDirection()\n"
384 "\tReturns the direction from the ray (in worldcoordinates) .\n";
385 PyObject* KX_RaySensor::PyGetRayDirection(PyObject* self, 
386                                PyObject* args, 
387                                PyObject* kwds)
388 {
389
390         MT_Vector3 dir = m_rayDirection;
391
392         PyObject* resultlist = PyList_New(3);
393         int index;
394         for (index=0;index<3;index++)
395         {
396                 PyList_SetItem(resultlist,index,PyFloat_FromDouble(dir[index]));
397         }
398         return resultlist;
399
400 }
401
402 char KX_RaySensor::GetHitNormal_doc[] = 
403 "getHitNormal()\n"
404 "\tReturns the normal (in worldcoordinates) of the object at the location where the object was hit by this ray.\n";
405 PyObject* KX_RaySensor::PyGetHitNormal(PyObject* self, 
406                                PyObject* args, 
407                                PyObject* kwds)
408 {
409         MT_Vector3 pos = m_hitNormal;
410
411         PyObject* resultlist = PyList_New(3);
412         int index;
413         for (index=0;index<3;index++)
414         {
415                 PyList_SetItem(resultlist,index,PyFloat_FromDouble(pos[index]));
416         }
417         return resultlist;
418
419 }
420
421
422
423 PyObject* KX_RaySensor::_getattr(const STR_String& attr) {
424         _getattr_up(SCA_ISensor);
425 }