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