2 * Add steering behaviors
5 * ***** BEGIN GPL LICENSE BLOCK *****
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.
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.
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.
21 * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
22 * All rights reserved.
24 * The Original Code is: all of this file.
26 * Contributor(s): none yet.
28 * ***** END GPL LICENSE BLOCK *****
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"
40 /* ------------------------------------------------------------------------- */
41 /* Native functions */
42 /* ------------------------------------------------------------------------- */
44 KX_SteeringActuator::KX_SteeringActuator(SCA_IObject *gameobj,
46 KX_GameObject *target,
47 KX_GameObject *navmesh,
52 bool isSelfTerminated,
54 KX_ObstacleSimulation* simulation,
57 bool enableVisualization)
58 : SCA_IActuator(gameobj, KX_ACT_STEERING),
63 m_acceleration(acceleration),
64 m_turnspeed(turnspeed),
65 m_simulation(simulation),
69 m_isSelfTerminated(isSelfTerminated),
70 m_enableVisualization(enableVisualization),
71 m_facingMode(facingmode),
74 m_pathUpdatePeriod(pathUpdatePeriod),
76 m_steerVec(MT_Vector3(0, 0, 0))
78 m_navmesh = static_cast<KX_NavMeshObject*>(navmesh);
80 m_navmesh->RegisterActuator(this);
82 m_target->RegisterActuator(this);
85 m_obstacle = m_simulation->GetObstacle((KX_GameObject*)gameobj);
86 KX_GameObject* parent = ((KX_GameObject*)gameobj)->GetParent();
87 if (m_facingMode>0 && parent)
89 m_parentlocalmat = parent->GetSGNode()->GetLocalOrientation();
92 m_parentlocalmat.setIdentity();
95 KX_SteeringActuator::~KX_SteeringActuator()
98 m_navmesh->UnregisterActuator(this);
100 m_target->UnregisterActuator(this);
103 CValue* KX_SteeringActuator::GetReplica()
105 KX_SteeringActuator* replica = new KX_SteeringActuator(*this);
106 // replication just copy the m_base pointer => common random generator
107 replica->ProcessReplica();
111 void KX_SteeringActuator::ProcessReplica()
114 m_target->RegisterActuator(this);
116 m_navmesh->RegisterActuator(this);
117 SCA_IActuator::ProcessReplica();
121 bool KX_SteeringActuator::UnlinkObject(SCA_IObject* clientobj)
123 if (clientobj == m_target)
128 else if (clientobj == m_navmesh)
136 void KX_SteeringActuator::Relink(CTR_Map<CTR_HashedPtr, void*> *obj_map)
138 void **h_obj = (*obj_map)[m_target];
141 m_target->UnregisterActuator(this);
142 m_target = (KX_GameObject*)(*h_obj);
143 m_target->RegisterActuator(this);
146 h_obj = (*obj_map)[m_navmesh];
149 m_navmesh->UnregisterActuator(this);
150 m_navmesh = (KX_NavMeshObject*)(*h_obj);
151 m_navmesh->RegisterActuator(this);
155 bool KX_SteeringActuator::Update(double curtime, bool frame)
159 double delta = curtime - m_updateTime;
160 m_updateTime = curtime;
162 if (m_posevent && !m_isActive)
165 m_pathUpdateTime = -1;
166 m_updateTime = curtime;
169 bool bNegativeEvent = IsNegativeEvent();
178 if (bNegativeEvent || !m_target)
179 return false; // do nothing on negative events
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;
187 m_steerVec = MT_Vector3(0, 0, 0);
188 bool apply_steerforce = false;
189 bool terminate = true;
192 case KX_STEERING_SEEK:
193 if (vectotarg2d.length2()>m_distance*m_distance)
196 m_steerVec = vectotarg;
197 m_steerVec.normalize();
198 apply_steerforce = true;
201 case KX_STEERING_FLEE:
202 if (vectotarg2d.length2()<m_distance*m_distance)
205 m_steerVec = -vectotarg;
206 m_steerVec.normalize();
207 apply_steerforce = true;
210 case KX_STEERING_PATHFOLLOWING:
211 if (m_navmesh && vectotarg.length2()>m_distance*m_distance)
215 static const MT_Scalar WAYPOINT_RADIUS(0.25);
217 if (m_pathUpdateTime<0 || (m_pathUpdatePeriod>=0 &&
218 curtime - m_pathUpdateTime>((double)m_pathUpdatePeriod/1000)))
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;
227 MT_Vector3 waypoint(&m_path[3*m_wayPointIdx]);
228 if ((waypoint-mypos).length2()<WAYPOINT_RADIUS*WAYPOINT_RADIUS)
231 if (m_wayPointIdx>=m_pathLen)
237 waypoint.setValue(&m_path[3*m_wayPointIdx]);
240 m_steerVec = waypoint - mypos;
241 apply_steerforce = true;
244 if (m_enableVisualization)
247 static const MT_Vector3 PATH_COLOR(1,0,0);
248 m_navmesh->DrawPath(m_path, m_pathLen, PATH_COLOR);
256 if (apply_steerforce)
258 bool isdyna = obj->IsDynamic();
261 if (!m_steerVec.fuzzyZero())
262 m_steerVec.normalize();
263 MT_Vector3 newvel = m_velocity*m_steerVec;
265 //adjust velocity to avoid obstacles
266 if (m_simulation && m_obstacle /*&& !newvel.fuzzyZero()*/)
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.));
276 HandleActorFace(newvel);
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);
287 MT_Vector3 movement = delta*newvel;
288 obj->ApplyMovement(movement, false);
293 if (m_simulation && m_obstacle)
295 m_obstacle->dvel[0] = 0.f;
296 m_obstacle->dvel[1] = 0.f;
301 if (terminate && m_isSelfTerminated)
308 const MT_Vector3& KX_SteeringActuator::GetSteeringVec()
310 static MT_Vector3 ZERO_VECTOR(0, 0, 0);
317 inline float vdot2(const float* a, const float* b)
319 return a[0]*b[0] + a[2]*b[2];
321 static bool barDistSqPointToTri(const float* p, const float* a, const float* b, const float* c)
323 float v0[3], v1[3], v2[3];
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);
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;
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;
344 inline void flipAxes(float* vec)
346 std::swap(vec[1],vec[2]);
349 static bool getNavmeshNormal(dtStatNavMesh* navmesh, const MT_Vector3& pos, MT_Vector3& normal)
351 static const float polyPickExt[3] = {2, 4, 2};
355 dtStatPolyRef sPolyRef = navmesh->findNearestPoly(spos, polyPickExt);
358 const dtStatPoly* p = navmesh->getPoly(sPolyRef-1);
359 const dtStatPolyDetail* pd = navmesh->getPolyDetail(sPolyRef-1);
361 float distMin = FLT_MAX;
363 for (int i = 0; i < pd->ntris; ++i)
365 const unsigned char* t = navmesh->getDetailTri(pd->tbase+i);
367 for (int j = 0; j < 3; ++j)
370 v[j] = navmesh->getVertex(p->v[t[j]]);
372 v[j] = navmesh->getDetailVertex(pd->vbase+(t[j]-p->nv));
374 float dist = barDistSqPointToTri(spos, v[0], v[1], v[2]);
384 const unsigned char* t = navmesh->getDetailTri(pd->tbase+idxMin);
386 for (int j = 0; j < 3; ++j)
389 v[j] = navmesh->getVertex(p->v[t[j]]);
391 v[j] = navmesh->getDetailVertex(pd->vbase+(t[j]-p->nv));
394 for (size_t j=0; j<3; j++)
395 tri[j].setValue(v[j][0],v[j][2],v[j][1]);
399 normal = b.cross(a).safe_normalized();
406 void KX_SteeringActuator::HandleActorFace(MT_Vector3& velocity)
408 if (m_facingMode==0 && (!m_navmesh || !m_normalUp))
410 KX_GameObject* curobj = (KX_GameObject*) GetParent();
411 MT_Vector3 dir = m_facingMode==0 ? curobj->NodeGetLocalOrientation().getColumn(1) : velocity;
415 MT_Vector3 up(0,0,1);
419 if (m_navmesh && m_normalUp)
421 dtStatNavMesh* navmesh = m_navmesh->GetNavMesh();
423 MT_Vector3 trpos = m_navmesh->TransformToLocalCoords(curobj->NodeGetWorldPosition());
424 if (getNavmeshNormal(navmesh, trpos, normal))
427 left = (dir.cross(up)).safe_normalized();
428 dir = (-left.cross(normal)).safe_normalized();
433 switch (m_facingMode)
437 left = dir.safe_normalized();
438 dir = -(left.cross(up)).safe_normalized();
443 left = (dir.cross(up)).safe_normalized();
449 left = up.safe_normalized();
450 up = dir.safe_normalized();
452 left = (dir.cross(up)).safe_normalized();
458 left = -dir.safe_normalized();
459 dir = -(left.cross(up)).safe_normalized();
464 left = (-dir.cross(up)).safe_normalized();
470 left = up.safe_normalized();
471 up = -dir.safe_normalized();
473 left = (dir.cross(up)).safe_normalized();
479 left[0], dir[0],up[0],
480 left[1], dir[1],up[1],
481 left[2], dir[2],up[2]
486 KX_GameObject* parentObject = curobj->GetParent();
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);
500 curobj->NodeSetLocalOrientation(mat);
505 #ifndef DISABLE_PYTHON
507 /* ------------------------------------------------------------------------- */
508 /* Python functions */
509 /* ------------------------------------------------------------------------- */
511 /* Integration hooks ------------------------------------------------------- */
512 PyTypeObject KX_SteeringActuator::Type = {
513 PyVarObject_HEAD_INIT(NULL, 0)
514 "KX_SteeringActuator",
515 sizeof(PyObjectPlus_Proxy),
524 Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,
529 &SCA_IActuator::Type,
534 PyMethodDef KX_SteeringActuator::Methods[] = {
535 {NULL,NULL} //Sentinel
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),
554 PyObject* KX_SteeringActuator::pyattr_get_target(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
556 KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self);
557 if (!actuator->m_target)
560 return actuator->m_target->GetProxy();
563 int KX_SteeringActuator::pyattr_set_target(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
565 KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self);
566 KX_GameObject *gameobj;
568 if (!ConvertPythonToGameObject(value, &gameobj, true, "actuator.object = value: KX_SteeringActuator"))
569 return PY_SET_ATTR_FAIL; // ConvertPythonToGameObject sets the error
571 if (actuator->m_target != NULL)
572 actuator->m_target->UnregisterActuator(actuator);
574 actuator->m_target = (KX_GameObject*) gameobj;
576 if (actuator->m_target)
577 actuator->m_target->RegisterActuator(actuator);
579 return PY_SET_ATTR_SUCCESS;
582 PyObject* KX_SteeringActuator::pyattr_get_navmesh(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
584 KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self);
585 if (!actuator->m_navmesh)
588 return actuator->m_navmesh->GetProxy();
591 int KX_SteeringActuator::pyattr_set_navmesh(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
593 KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self);
594 KX_GameObject *gameobj;
596 if (!ConvertPythonToGameObject(value, &gameobj, true, "actuator.object = value: KX_SteeringActuator"))
597 return PY_SET_ATTR_FAIL; // ConvertPythonToGameObject sets the error
599 if (!PyObject_TypeCheck(value, &KX_NavMeshObject::Type))
601 PyErr_Format(PyExc_TypeError, "KX_NavMeshObject is expected");
602 return PY_SET_ATTR_FAIL;
605 if (actuator->m_navmesh != NULL)
606 actuator->m_navmesh->UnregisterActuator(actuator);
608 actuator->m_navmesh = static_cast<KX_NavMeshObject*>(gameobj);
610 if (actuator->m_navmesh)
611 actuator->m_navmesh->RegisterActuator(actuator);
613 return PY_SET_ATTR_SUCCESS;
616 PyObject* KX_SteeringActuator::pyattr_get_steeringVec(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
618 KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self);
619 const MT_Vector3& steeringVec = actuator->GetSteeringVec();
620 return PyObjectFrom(steeringVec);
623 #endif // DISABLE_PYTHON