- fixed bug in steering actuator: calculate 2d distance to target for seeking and...
[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
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                                                                         bool enableVisualization)        : 
59         SCA_IActuator(gameobj, KX_ACT_STEERING),
60         m_mode(mode),
61         m_target(target),
62         m_distance(distance),
63         m_velocity(velocity),
64         m_acceleration(acceleration),
65         m_turnspeed(turnspeed),
66         m_isSelfTerminated(isSelfTerminated),
67         m_pathUpdatePeriod(pathUpdatePeriod),
68         m_updateTime(0),
69         m_isActive(false),      
70         m_simulation(simulation),       
71         m_enableVisualization(enableVisualization),
72         m_obstacle(NULL),
73         m_pathLen(0),
74         m_wayPointIdx(-1)
75 {
76         m_navmesh = static_cast<KX_NavMeshObject*>(navmesh);
77         if (m_navmesh)
78                 m_navmesh->RegisterActuator(this);
79         if (m_target)
80                 m_target->RegisterActuator(this);
81         
82         if (m_simulation)
83                 m_obstacle = m_simulation->GetObstacle((KX_GameObject*)gameobj);
84
85
86 KX_SteeringActuator::~KX_SteeringActuator()
87 {
88         if (m_navmesh)
89                 m_navmesh->UnregisterActuator(this);
90         if (m_target)
91                 m_target->UnregisterActuator(this);
92
93
94 CValue* KX_SteeringActuator::GetReplica()
95 {
96         KX_SteeringActuator* replica = new KX_SteeringActuator(*this);
97         // replication just copy the m_base pointer => common random generator
98         replica->ProcessReplica();
99         return replica;
100 }
101
102 void KX_SteeringActuator::ProcessReplica()
103 {
104         if (m_target)
105                 m_target->RegisterActuator(this);
106         if (m_navmesh)
107                 m_navmesh->RegisterActuator(this);
108         SCA_IActuator::ProcessReplica();
109 }
110
111
112 bool KX_SteeringActuator::UnlinkObject(SCA_IObject* clientobj)
113 {
114         if (clientobj == m_target)
115         {
116                 m_target = NULL;
117                 return true;
118         }
119         else if (clientobj == m_navmesh)
120         {
121                 m_navmesh = NULL;
122                 return true;
123         }
124         return false;
125 }
126
127 void KX_SteeringActuator::Relink(GEN_Map<GEN_HashedPtr, void*> *obj_map)
128 {
129         void **h_obj = (*obj_map)[m_target];
130         if (h_obj) {
131                 if (m_target)
132                         m_target->UnregisterActuator(this);
133                 m_target = (KX_GameObject*)(*h_obj);
134                 m_target->RegisterActuator(this);
135         }
136
137         h_obj = (*obj_map)[m_navmesh];
138         if (h_obj) {
139                 if (m_navmesh)
140                         m_navmesh->UnregisterActuator(this);
141                 m_navmesh = (KX_NavMeshObject*)(*h_obj);
142                 m_navmesh->RegisterActuator(this);
143         }
144 }
145
146 bool KX_SteeringActuator::Update(double curtime, bool frame)
147 {
148         if (frame)
149         {
150                 double delta =  curtime - m_updateTime;
151                 m_updateTime = curtime;
152                 
153                 if (m_posevent && !m_isActive)
154                 {
155                         delta = 0;
156                         m_pathUpdateTime = -1;
157                         m_updateTime = curtime;
158                         m_isActive = true;
159                 }
160                 bool bNegativeEvent = IsNegativeEvent();
161                 if (bNegativeEvent)
162                         m_isActive = false;
163
164                 RemoveAllEvents();
165
166                 if (!delta)
167                         return true;
168
169                 if (bNegativeEvent || !m_target)
170                         return false; // do nothing on negative events
171
172                 KX_GameObject *obj = (KX_GameObject*) GetParent();
173                 const MT_Point3& mypos = obj->NodeGetWorldPosition();
174                 const MT_Point3& targpos = m_target->NodeGetWorldPosition();
175                 MT_Vector3 vectotarg = targpos - mypos;
176                 MT_Vector3 vectotarg2d = vectotarg;
177                 vectotarg2d.z() = 0;
178                 MT_Vector3 steervec = MT_Vector3(0, 0, 0);
179                 bool apply_steerforce = false;
180                 bool terminate = true;
181
182                 switch (m_mode) {
183                         case KX_STEERING_SEEK:
184                                 if (vectotarg2d.length2()>m_distance*m_distance)
185                                 {
186                                         terminate = false;
187                                         steervec = vectotarg;
188                                         steervec.normalize();
189                                         apply_steerforce = true;
190                                 }
191                                 break;
192                         case KX_STEERING_FLEE:
193                                 if (vectotarg2d.length2()<m_distance*m_distance)
194                                 {
195                                         terminate = false;
196                                         steervec = -vectotarg;
197                                         steervec.normalize();
198                                         apply_steerforce = true;
199                                 }
200                                 break;
201                         case KX_STEERING_PATHFOLLOWING:
202                                 if (m_navmesh && vectotarg.length2()>m_distance*m_distance)
203                                 {
204                                         terminate = false;
205
206                                         static const MT_Scalar WAYPOINT_RADIUS(0.25);
207
208                                         if (m_pathUpdateTime<0 || (m_pathUpdatePeriod>=0 && 
209                                                                                                 curtime - m_pathUpdateTime>((double)m_pathUpdatePeriod/1000)))
210                                         {
211                                                 m_pathUpdateTime = curtime;
212                                                 m_pathLen = m_navmesh->FindPath(mypos, targpos, m_path, MAX_PATH_LENGTH);
213                                                 m_wayPointIdx = m_pathLen > 1 ? 1 : -1;
214                                         }
215
216                                         if (m_wayPointIdx>0)
217                                         {
218                                                 MT_Vector3 waypoint(&m_path[3*m_wayPointIdx]);
219                                                 if ((waypoint-mypos).length2()<WAYPOINT_RADIUS*WAYPOINT_RADIUS)
220                                                 {
221                                                         m_wayPointIdx++;
222                                                         if (m_wayPointIdx>=m_pathLen)
223                                                         {
224                                                                 m_wayPointIdx = -1;
225                                                                 terminate = true;
226                                                         }
227                                                         else
228                                                                 waypoint.setValue(&m_path[3*m_wayPointIdx]);
229                                                 }
230
231                                                 steervec = waypoint - mypos;
232                                                 apply_steerforce = true;
233
234                                                 
235                                                 if (m_enableVisualization)
236                                                 {
237                                                         //debug draw
238                                                         static const MT_Vector3 PATH_COLOR(1,0,0);
239                                                         m_navmesh->DrawPath(m_path, m_pathLen, PATH_COLOR);
240                                                 }
241                                         }       
242                                         
243                                 }
244                                 break;
245                 }
246
247                 if (apply_steerforce)
248                 {
249                         bool isdyna = obj->IsDynamic();
250                         if (isdyna)
251                                 steervec.z() = 0;
252                         if (!steervec.fuzzyZero())
253                                 steervec.normalize();
254                         MT_Vector3 newvel = m_velocity*steervec;
255
256                         //adjust velocity to avoid obstacles
257                         if (m_simulation && m_obstacle && !newvel.fuzzyZero())
258                         {
259                                 if (m_enableVisualization)
260                                         KX_RasterizerDrawDebugLine(mypos, mypos + newvel, MT_Vector3(1.,0.,0.));
261                                 m_simulation->AdjustObstacleVelocity(m_obstacle, m_mode!=KX_STEERING_PATHFOLLOWING ? m_navmesh : NULL, 
262                                                                 newvel, m_acceleration*delta, m_turnspeed/180.0f*M_PI*delta);
263                                 if (m_enableVisualization)
264                                         KX_RasterizerDrawDebugLine(mypos, mypos + newvel, MT_Vector3(0.,1.,0.));
265                         }
266
267                         if (isdyna)
268                         {
269                                 //temporary solution: set 2D steering velocity directly to obj
270                                 //correct way is to apply physical force
271                                 MT_Vector3 curvel = obj->GetLinearVelocity();
272                                 newvel.z() = curvel.z();                        
273                                 obj->setLinearVelocity(newvel, false);
274                         }
275                         else
276                         {
277                                 MT_Vector3 movement = delta*newvel;
278                                 obj->ApplyMovement(movement, false);
279                         }
280                 }
281
282                 if (terminate && m_isSelfTerminated)
283                         return false;
284         }
285
286         return true;
287 }
288
289 #ifndef DISABLE_PYTHON
290
291 /* ------------------------------------------------------------------------- */
292 /* Python functions                                                          */
293 /* ------------------------------------------------------------------------- */
294
295 /* Integration hooks ------------------------------------------------------- */
296 PyTypeObject KX_SteeringActuator::Type = {
297         PyVarObject_HEAD_INIT(NULL, 0)
298         "KX_SteeringActuator",
299         sizeof(PyObjectPlus_Proxy),
300         0,
301         py_base_dealloc,
302         0,
303         0,
304         0,
305         0,
306         py_base_repr,
307         0,0,0,0,0,0,0,0,0,
308         Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,
309         0,0,0,0,0,0,0,
310         Methods,
311         0,
312         0,
313         &SCA_IActuator::Type,
314         0,0,0,0,0,0,
315         py_base_new
316 };
317
318 PyMethodDef KX_SteeringActuator::Methods[] = {
319         {NULL,NULL} //Sentinel
320 };
321
322 PyAttributeDef KX_SteeringActuator::Attributes[] = {
323         KX_PYATTRIBUTE_INT_RW("behaviour", KX_STEERING_NODEF+1, KX_STEERING_MAX-1, true, KX_SteeringActuator, m_mode),
324         KX_PYATTRIBUTE_RW_FUNCTION("target", KX_SteeringActuator, pyattr_get_target, pyattr_set_target),
325         KX_PYATTRIBUTE_RW_FUNCTION("navmesh", KX_SteeringActuator, pyattr_get_navmesh, pyattr_set_navmesh),
326         KX_PYATTRIBUTE_FLOAT_RW("distance", 0.0f, 1000.0f, KX_SteeringActuator, m_distance),
327         KX_PYATTRIBUTE_FLOAT_RW("velocity", 0.0f, 1000.0f, KX_SteeringActuator, m_velocity),
328         KX_PYATTRIBUTE_FLOAT_RW("acceleration", 0.0f, 1000.0f, KX_SteeringActuator, m_acceleration),
329         KX_PYATTRIBUTE_FLOAT_RW("turnspeed", 0.0f, 720.0f, KX_SteeringActuator, m_turnspeed),
330         KX_PYATTRIBUTE_BOOL_RW("selfterminated", KX_SteeringActuator, m_isSelfTerminated),
331         KX_PYATTRIBUTE_BOOL_RW("enableVisualization", KX_SteeringActuator, m_enableVisualization),
332         KX_PYATTRIBUTE_INT_RW("pathUpdatePeriod", -1, 100000, true, KX_SteeringActuator, m_pathUpdatePeriod),
333         { NULL }        //Sentinel
334 };
335
336 PyObject* KX_SteeringActuator::pyattr_get_target(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
337 {
338         KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self);
339         if (!actuator->m_target)        
340                 Py_RETURN_NONE;
341         else
342                 return actuator->m_target->GetProxy();
343 }
344
345 int KX_SteeringActuator::pyattr_set_target(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
346 {
347         KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self);
348         KX_GameObject *gameobj;
349
350         if (!ConvertPythonToGameObject(value, &gameobj, true, "actuator.object = value: KX_SteeringActuator"))
351                 return PY_SET_ATTR_FAIL; // ConvertPythonToGameObject sets the error
352
353         if (actuator->m_target != NULL)
354                 actuator->m_target->UnregisterActuator(actuator);       
355
356         actuator->m_target = (KX_GameObject*) gameobj;
357
358         if (actuator->m_target)
359                 actuator->m_target->RegisterActuator(actuator);
360
361         return PY_SET_ATTR_SUCCESS;
362 }
363
364 PyObject* KX_SteeringActuator::pyattr_get_navmesh(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
365 {
366         KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self);
367         if (!actuator->m_navmesh)       
368                 Py_RETURN_NONE;
369         else
370                 return actuator->m_navmesh->GetProxy();
371 }
372
373 int KX_SteeringActuator::pyattr_set_navmesh(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
374 {
375         KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self);
376         KX_GameObject *gameobj;
377
378         if (!ConvertPythonToGameObject(value, &gameobj, true, "actuator.object = value: KX_SteeringActuator"))
379                 return PY_SET_ATTR_FAIL; // ConvertPythonToGameObject sets the error
380
381         if (!PyObject_TypeCheck(value, &KX_NavMeshObject::Type))
382         {
383                 PyErr_Format(PyExc_TypeError, "KX_NavMeshObject is expected");
384                 return PY_SET_ATTR_FAIL;
385         }
386
387         if (actuator->m_navmesh != NULL)
388                 actuator->m_navmesh->UnregisterActuator(actuator);      
389
390         actuator->m_navmesh = static_cast<KX_NavMeshObject*>(gameobj);
391
392         if (actuator->m_navmesh)
393                 actuator->m_navmesh->RegisterActuator(actuator);
394
395         return PY_SET_ATTR_SUCCESS;
396 }
397
398 #endif // DISABLE_PYTHON
399
400 /* eof */
401