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