synched with trunk at revision 36569
[blender.git] / source / gameengine / Ketsji / KX_SteeringActuator.cpp
1 /**
2 * Add steering behaviors
3 *
4 * $Id$
5 *
6 * ***** BEGIN GPL 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, 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 LICENSE BLOCK *****
33 */
34
35 #include "BLI_math.h"
36 #include "KX_SteeringActuator.h"
37 #include "KX_GameObject.h"
38 #include "KX_NavMeshObject.h"
39 #include "KX_ObstacleSimulation.h"
40 #include "KX_PythonInit.h"
41 #include "KX_PyMath.h"
42 #include "Recast.h"
43
44 /* ------------------------------------------------------------------------- */
45 /* Native functions                                                          */
46 /* ------------------------------------------------------------------------- */
47
48 KX_SteeringActuator::KX_SteeringActuator(SCA_IObject *gameobj, 
49                                                                         int mode,
50                                                                         KX_GameObject *target,
51                                                                         KX_GameObject *navmesh,
52                                                                         float distance,
53                                                                         float velocity, 
54                                                                         float acceleration,                                                                     
55                                                                         float turnspeed,
56                                                                         bool  isSelfTerminated,
57                                                                         int pathUpdatePeriod,
58                                                                         KX_ObstacleSimulation* simulation,
59                                                                         short facingmode,
60                                                                         bool normalup,
61                                                                         bool enableVisualization)        : 
62         SCA_IActuator(gameobj, KX_ACT_STEERING),
63         m_mode(mode),
64         m_target(target),
65         m_distance(distance),
66         m_velocity(velocity),
67         m_acceleration(acceleration),
68         m_turnspeed(turnspeed),
69         m_isSelfTerminated(isSelfTerminated),
70         m_pathUpdatePeriod(pathUpdatePeriod),
71         m_updateTime(0),
72         m_isActive(false),      
73         m_simulation(simulation),       
74         m_enableVisualization(enableVisualization),
75         m_facingMode(facingmode),
76         m_normalUp(normalup),
77         m_obstacle(NULL),
78         m_pathLen(0),
79         m_wayPointIdx(-1),
80         m_steerVec(MT_Vector3(0, 0, 0))
81 {
82         m_navmesh = static_cast<KX_NavMeshObject*>(navmesh);
83         if (m_navmesh)
84                 m_navmesh->RegisterActuator(this);
85         if (m_target)
86                 m_target->RegisterActuator(this);
87         
88         if (m_simulation)
89                 m_obstacle = m_simulation->GetObstacle((KX_GameObject*)gameobj);
90         KX_GameObject* parent = ((KX_GameObject*)gameobj)->GetParent();
91         if (m_facingMode>0 && parent)
92         {
93                 m_parentlocalmat = parent->GetSGNode()->GetLocalOrientation();
94         }
95         else
96                 m_parentlocalmat.setIdentity();
97
98
99 KX_SteeringActuator::~KX_SteeringActuator()
100 {
101         if (m_navmesh)
102                 m_navmesh->UnregisterActuator(this);
103         if (m_target)
104                 m_target->UnregisterActuator(this);
105
106
107 CValue* KX_SteeringActuator::GetReplica()
108 {
109         KX_SteeringActuator* replica = new KX_SteeringActuator(*this);
110         // replication just copy the m_base pointer => common random generator
111         replica->ProcessReplica();
112         return replica;
113 }
114
115 void KX_SteeringActuator::ProcessReplica()
116 {
117         if (m_target)
118                 m_target->RegisterActuator(this);
119         if (m_navmesh)
120                 m_navmesh->RegisterActuator(this);
121         SCA_IActuator::ProcessReplica();
122 }
123
124
125 bool KX_SteeringActuator::UnlinkObject(SCA_IObject* clientobj)
126 {
127         if (clientobj == m_target)
128         {
129                 m_target = NULL;
130                 return true;
131         }
132         else if (clientobj == m_navmesh)
133         {
134                 m_navmesh = NULL;
135                 return true;
136         }
137         return false;
138 }
139
140 void KX_SteeringActuator::Relink(CTR_Map<CTR_HashedPtr, void*> *obj_map)
141 {
142         void **h_obj = (*obj_map)[m_target];
143         if (h_obj) {
144                 if (m_target)
145                         m_target->UnregisterActuator(this);
146                 m_target = (KX_GameObject*)(*h_obj);
147                 m_target->RegisterActuator(this);
148         }
149
150         h_obj = (*obj_map)[m_navmesh];
151         if (h_obj) {
152                 if (m_navmesh)
153                         m_navmesh->UnregisterActuator(this);
154                 m_navmesh = (KX_NavMeshObject*)(*h_obj);
155                 m_navmesh->RegisterActuator(this);
156         }
157 }
158
159 bool KX_SteeringActuator::Update(double curtime, bool frame)
160 {
161         if (frame)
162         {
163                 double delta =  curtime - m_updateTime;
164                 m_updateTime = curtime;
165                 
166                 if (m_posevent && !m_isActive)
167                 {
168                         delta = 0;
169                         m_pathUpdateTime = -1;
170                         m_updateTime = curtime;
171                         m_isActive = true;
172                 }
173                 bool bNegativeEvent = IsNegativeEvent();
174                 if (bNegativeEvent)
175                         m_isActive = false;
176
177                 RemoveAllEvents();
178
179                 if (!delta)
180                         return true;
181
182                 if (bNegativeEvent || !m_target)
183                         return false; // do nothing on negative events
184
185                 KX_GameObject *obj = (KX_GameObject*) GetParent();
186                 const MT_Point3& mypos = obj->NodeGetWorldPosition();
187                 const MT_Point3& targpos = m_target->NodeGetWorldPosition();
188                 MT_Vector3 vectotarg = targpos - mypos;
189                 MT_Vector3 vectotarg2d = vectotarg;
190                 vectotarg2d.z() = 0;
191                 m_steerVec = MT_Vector3(0, 0, 0);
192                 bool apply_steerforce = false;
193                 bool terminate = true;
194
195                 switch (m_mode) {
196                         case KX_STEERING_SEEK:
197                                 if (vectotarg2d.length2()>m_distance*m_distance)
198                                 {
199                                         terminate = false;
200                                         m_steerVec = vectotarg;
201                                         m_steerVec.normalize();
202                                         apply_steerforce = true;
203                                 }
204                                 break;
205                         case KX_STEERING_FLEE:
206                                 if (vectotarg2d.length2()<m_distance*m_distance)
207                                 {
208                                         terminate = false;
209                                         m_steerVec = -vectotarg;
210                                         m_steerVec.normalize();
211                                         apply_steerforce = true;
212                                 }
213                                 break;
214                         case KX_STEERING_PATHFOLLOWING:
215                                 if (m_navmesh && vectotarg.length2()>m_distance*m_distance)
216                                 {
217                                         terminate = false;
218
219                                         static const MT_Scalar WAYPOINT_RADIUS(0.25);
220
221                                         if (m_pathUpdateTime<0 || (m_pathUpdatePeriod>=0 && 
222                                                                                                 curtime - m_pathUpdateTime>((double)m_pathUpdatePeriod/1000)))
223                                         {
224                                                 m_pathUpdateTime = curtime;
225                                                 m_pathLen = m_navmesh->FindPath(mypos, targpos, m_path, MAX_PATH_LENGTH);
226                                                 m_wayPointIdx = m_pathLen > 1 ? 1 : -1;
227                                         }
228
229                                         if (m_wayPointIdx>0)
230                                         {
231                                                 MT_Vector3 waypoint(&m_path[3*m_wayPointIdx]);
232                                                 if ((waypoint-mypos).length2()<WAYPOINT_RADIUS*WAYPOINT_RADIUS)
233                                                 {
234                                                         m_wayPointIdx++;
235                                                         if (m_wayPointIdx>=m_pathLen)
236                                                         {
237                                                                 m_wayPointIdx = -1;
238                                                                 terminate = true;
239                                                         }
240                                                         else
241                                                                 waypoint.setValue(&m_path[3*m_wayPointIdx]);
242                                                 }
243
244                                                 m_steerVec = waypoint - mypos;
245                                                 apply_steerforce = true;
246
247                                                 
248                                                 if (m_enableVisualization)
249                                                 {
250                                                         //debug draw
251                                                         static const MT_Vector3 PATH_COLOR(1,0,0);
252                                                         m_navmesh->DrawPath(m_path, m_pathLen, PATH_COLOR);
253                                                 }
254                                         }       
255                                         
256                                 }
257                                 break;
258                 }
259
260                 if (apply_steerforce)
261                 {
262                         bool isdyna = obj->IsDynamic();
263                         if (isdyna)
264                                 m_steerVec.z() = 0;
265                         if (!m_steerVec.fuzzyZero())
266                                 m_steerVec.normalize();
267                         MT_Vector3 newvel = m_velocity*m_steerVec;
268
269                         //adjust velocity to avoid obstacles
270                         if (m_simulation && m_obstacle /*&& !newvel.fuzzyZero()*/)
271                         {
272                                 if (m_enableVisualization)
273                                         KX_RasterizerDrawDebugLine(mypos, mypos + newvel, MT_Vector3(1.,0.,0.));
274                                 m_simulation->AdjustObstacleVelocity(m_obstacle, m_mode!=KX_STEERING_PATHFOLLOWING ? m_navmesh : NULL, 
275                                                                 newvel, m_acceleration*delta, m_turnspeed/180.0f*M_PI*delta);
276                                 if (m_enableVisualization)
277                                         KX_RasterizerDrawDebugLine(mypos, mypos + newvel, MT_Vector3(0.,1.,0.));
278                         }
279
280                         HandleActorFace(newvel);
281                         if (isdyna)
282                         {
283                                 //temporary solution: set 2D steering velocity directly to obj
284                                 //correct way is to apply physical force
285                                 MT_Vector3 curvel = obj->GetLinearVelocity();
286                                 newvel.z() = curvel.z();                        
287                                 obj->setLinearVelocity(newvel, false);
288                         }
289                         else
290                         {
291                                 MT_Vector3 movement = delta*newvel;
292                                 obj->ApplyMovement(movement, false);
293                         }
294                 }
295                 else
296                 {
297                         if (m_simulation && m_obstacle)
298                         {
299                                 m_obstacle->dvel[0] = 0.f;
300                                 m_obstacle->dvel[1] = 0.f;
301                         }
302                         
303                 }
304
305                 if (terminate && m_isSelfTerminated)
306                         return false;
307         }
308
309         return true;
310 }
311
312 const MT_Vector3& KX_SteeringActuator::GetSteeringVec()
313 {
314         static MT_Vector3 ZERO_VECTOR(0, 0, 0);
315         if (m_isActive)
316                 return m_steerVec;
317         else
318                 return ZERO_VECTOR;
319 }
320
321 inline float vdot2(const float* a, const float* b)
322 {
323         return a[0]*b[0] + a[2]*b[2];
324 }
325 static bool barDistSqPointToTri(const float* p, const float* a, const float* b, const float* c)
326 {
327         float v0[3], v1[3], v2[3];
328         vsub(v0, c,a);
329         vsub(v1, b,a);
330         vsub(v2, p,a);
331
332         const float dot00 = vdot2(v0, v0);
333         const float dot01 = vdot2(v0, v1);
334         const float dot02 = vdot2(v0, v2);
335         const float dot11 = vdot2(v1, v1);
336         const float dot12 = vdot2(v1, v2);
337
338         // Compute barycentric coordinates
339         float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
340         float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
341         float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
342
343         float ud = u<0.f ? -u : (u>1.f ? u-1.f : 0.f);
344         float vd = v<0.f ? -v : (v>1.f ? v-1.f : 0.f);
345         return ud*ud+vd*vd ;
346 }
347
348 inline void flipAxes(float* vec)
349 {
350         std::swap(vec[1],vec[2]);
351 }
352
353 static bool getNavmeshNormal(dtStatNavMesh* navmesh, const MT_Vector3& pos, MT_Vector3& normal)
354 {
355         static const float polyPickExt[3] = {2, 4, 2};
356         float spos[3];
357         pos.getValue(spos);     
358         flipAxes(spos); 
359         dtStatPolyRef sPolyRef = navmesh->findNearestPoly(spos, polyPickExt);
360         if (sPolyRef == 0)
361                 return false;
362         const dtStatPoly* p = navmesh->getPoly(sPolyRef-1);
363         const dtStatPolyDetail* pd = navmesh->getPolyDetail(sPolyRef-1);
364
365         float distMin = FLT_MAX;
366         int idxMin = -1;
367         for (int i = 0; i < pd->ntris; ++i)
368         {
369                 const unsigned char* t = navmesh->getDetailTri(pd->tbase+i);
370                 const float* v[3];
371                 for (int j = 0; j < 3; ++j)
372                 {
373                         if (t[j] < p->nv)
374                                 v[j] = navmesh->getVertex(p->v[t[j]]);
375                         else
376                                 v[j] = navmesh->getDetailVertex(pd->vbase+(t[j]-p->nv));
377                 }
378                 float dist = barDistSqPointToTri(spos, v[0], v[1], v[2]);
379                 if (dist<distMin)
380                 {
381                         distMin = dist;
382                         idxMin = i;
383                 }                       
384         }
385
386         if (idxMin>=0)
387         {
388                 const unsigned char* t = navmesh->getDetailTri(pd->tbase+idxMin);
389                 const float* v[3];
390                 for (int j = 0; j < 3; ++j)
391                 {
392                         if (t[j] < p->nv)
393                                 v[j] = navmesh->getVertex(p->v[t[j]]);
394                         else
395                                 v[j] = navmesh->getDetailVertex(pd->vbase+(t[j]-p->nv));
396                 }
397                 MT_Vector3 tri[3];
398                 for (size_t j=0; j<3; j++)
399                         tri[j].setValue(v[j][0],v[j][2],v[j][1]);
400                 MT_Vector3 a,b;
401                 a = tri[1]-tri[0];
402                 b = tri[2]-tri[0];
403                 normal = b.cross(a).safe_normalized();
404                 return true;
405         }
406
407         return false;
408 }
409
410 void KX_SteeringActuator::HandleActorFace(MT_Vector3& velocity)
411 {
412         if (m_facingMode==0 && (!m_navmesh || !m_normalUp))
413                 return;
414         KX_GameObject* curobj = (KX_GameObject*) GetParent();
415         MT_Vector3 dir = m_facingMode==0 ?  curobj->NodeGetLocalOrientation().getColumn(1) : velocity;
416         if (dir.fuzzyZero())
417                 return; 
418         dir.normalize();
419         MT_Vector3 up(0,0,1);
420         MT_Vector3 left;
421         MT_Matrix3x3 mat;
422         
423         if (m_navmesh && m_normalUp)
424         {
425                 dtStatNavMesh* navmesh =  m_navmesh->GetNavMesh();
426                 MT_Vector3 normal;
427                 MT_Vector3 trpos = m_navmesh->TransformToLocalCoords(curobj->NodeGetWorldPosition());
428                 if (getNavmeshNormal(navmesh, trpos, normal))
429                 {
430
431                         left = (dir.cross(up)).safe_normalized();
432                         dir = (-left.cross(normal)).safe_normalized();                  
433                         up = normal;
434                 }
435         }
436
437         switch (m_facingMode)
438         {
439         case 1: // TRACK X
440                 {
441                         left  = dir.safe_normalized();
442                         dir = -(left.cross(up)).safe_normalized();
443                         break;
444                 };
445         case 2: // TRACK Y
446                 {
447                         left  = (dir.cross(up)).safe_normalized();
448                         break;
449                 }
450
451         case 3: // track Z
452                 {
453                         left = up.safe_normalized();
454                         up = dir.safe_normalized();
455                         dir = left;
456                         left  = (dir.cross(up)).safe_normalized();
457                         break;
458                 }
459
460         case 4: // TRACK -X
461                 {
462                         left  = -dir.safe_normalized();
463                         dir = -(left.cross(up)).safe_normalized();
464                         break;
465                 };
466         case 5: // TRACK -Y
467                 {
468                         left  = (-dir.cross(up)).safe_normalized();
469                         dir = -dir;
470                         break;
471                 }
472         case 6: // track -Z
473                 {
474                         left = up.safe_normalized();
475                         up = -dir.safe_normalized();
476                         dir = left;
477                         left  = (dir.cross(up)).safe_normalized();
478                         break;
479                 }
480         }
481
482         mat.setValue (
483                 left[0], dir[0],up[0], 
484                 left[1], dir[1],up[1],
485                 left[2], dir[2],up[2]
486         );
487
488         
489         
490         KX_GameObject* parentObject = curobj->GetParent();
491         if(parentObject)
492         { 
493                 MT_Point3 localpos;
494                 localpos = curobj->GetSGNode()->GetLocalPosition();
495                 MT_Matrix3x3 parentmatinv;
496                 parentmatinv = parentObject->NodeGetWorldOrientation ().inverse ();                             
497                 mat = parentmatinv * mat;
498                 mat = m_parentlocalmat * mat;
499                 curobj->NodeSetLocalOrientation(mat);
500                 curobj->NodeSetLocalPosition(localpos);
501         }
502         else
503         {
504                 curobj->NodeSetLocalOrientation(mat);
505         }
506
507 }
508
509 #ifndef DISABLE_PYTHON
510
511 /* ------------------------------------------------------------------------- */
512 /* Python functions                                                          */
513 /* ------------------------------------------------------------------------- */
514
515 /* Integration hooks ------------------------------------------------------- */
516 PyTypeObject KX_SteeringActuator::Type = {
517         PyVarObject_HEAD_INIT(NULL, 0)
518         "KX_SteeringActuator",
519         sizeof(PyObjectPlus_Proxy),
520         0,
521         py_base_dealloc,
522         0,
523         0,
524         0,
525         0,
526         py_base_repr,
527         0,0,0,0,0,0,0,0,0,
528         Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,
529         0,0,0,0,0,0,0,
530         Methods,
531         0,
532         0,
533         &SCA_IActuator::Type,
534         0,0,0,0,0,0,
535         py_base_new
536 };
537
538 PyMethodDef KX_SteeringActuator::Methods[] = {
539         {NULL,NULL} //Sentinel
540 };
541
542 PyAttributeDef KX_SteeringActuator::Attributes[] = {
543         KX_PYATTRIBUTE_INT_RW("behaviour", KX_STEERING_NODEF+1, KX_STEERING_MAX-1, true, KX_SteeringActuator, m_mode),
544         KX_PYATTRIBUTE_RW_FUNCTION("target", KX_SteeringActuator, pyattr_get_target, pyattr_set_target),
545         KX_PYATTRIBUTE_RW_FUNCTION("navmesh", KX_SteeringActuator, pyattr_get_navmesh, pyattr_set_navmesh),
546         KX_PYATTRIBUTE_FLOAT_RW("distance", 0.0f, 1000.0f, KX_SteeringActuator, m_distance),
547         KX_PYATTRIBUTE_FLOAT_RW("velocity", 0.0f, 1000.0f, KX_SteeringActuator, m_velocity),
548         KX_PYATTRIBUTE_FLOAT_RW("acceleration", 0.0f, 1000.0f, KX_SteeringActuator, m_acceleration),
549         KX_PYATTRIBUTE_FLOAT_RW("turnspeed", 0.0f, 720.0f, KX_SteeringActuator, m_turnspeed),
550         KX_PYATTRIBUTE_BOOL_RW("selfterminated", KX_SteeringActuator, m_isSelfTerminated),
551         KX_PYATTRIBUTE_BOOL_RW("enableVisualization", KX_SteeringActuator, m_enableVisualization),
552         KX_PYATTRIBUTE_RO_FUNCTION("steeringVec", KX_SteeringActuator, pyattr_get_steeringVec), 
553         KX_PYATTRIBUTE_SHORT_RW("facingMode", 0, 6, true, KX_SteeringActuator, m_facingMode),
554         KX_PYATTRIBUTE_INT_RW("pathUpdatePeriod", -1, 100000, true, KX_SteeringActuator, m_pathUpdatePeriod),
555         { NULL }        //Sentinel
556 };
557
558 PyObject* KX_SteeringActuator::pyattr_get_target(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
559 {
560         KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self);
561         if (!actuator->m_target)        
562                 Py_RETURN_NONE;
563         else
564                 return actuator->m_target->GetProxy();
565 }
566
567 int KX_SteeringActuator::pyattr_set_target(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
568 {
569         KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self);
570         KX_GameObject *gameobj;
571
572         if (!ConvertPythonToGameObject(value, &gameobj, true, "actuator.object = value: KX_SteeringActuator"))
573                 return PY_SET_ATTR_FAIL; // ConvertPythonToGameObject sets the error
574
575         if (actuator->m_target != NULL)
576                 actuator->m_target->UnregisterActuator(actuator);       
577
578         actuator->m_target = (KX_GameObject*) gameobj;
579
580         if (actuator->m_target)
581                 actuator->m_target->RegisterActuator(actuator);
582
583         return PY_SET_ATTR_SUCCESS;
584 }
585
586 PyObject* KX_SteeringActuator::pyattr_get_navmesh(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
587 {
588         KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self);
589         if (!actuator->m_navmesh)       
590                 Py_RETURN_NONE;
591         else
592                 return actuator->m_navmesh->GetProxy();
593 }
594
595 int KX_SteeringActuator::pyattr_set_navmesh(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
596 {
597         KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self);
598         KX_GameObject *gameobj;
599
600         if (!ConvertPythonToGameObject(value, &gameobj, true, "actuator.object = value: KX_SteeringActuator"))
601                 return PY_SET_ATTR_FAIL; // ConvertPythonToGameObject sets the error
602
603         if (!PyObject_TypeCheck(value, &KX_NavMeshObject::Type))
604         {
605                 PyErr_Format(PyExc_TypeError, "KX_NavMeshObject is expected");
606                 return PY_SET_ATTR_FAIL;
607         }
608
609         if (actuator->m_navmesh != NULL)
610                 actuator->m_navmesh->UnregisterActuator(actuator);      
611
612         actuator->m_navmesh = static_cast<KX_NavMeshObject*>(gameobj);
613
614         if (actuator->m_navmesh)
615                 actuator->m_navmesh->RegisterActuator(actuator);
616
617         return PY_SET_ATTR_SUCCESS;
618 }
619
620 PyObject* KX_SteeringActuator::pyattr_get_steeringVec(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
621 {
622         KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self);
623         const MT_Vector3& steeringVec = actuator->GetSteeringVec();
624         return PyObjectFrom(steeringVec);
625 }
626
627 #endif // DISABLE_PYTHON
628
629 /* eof */
630