diff options
Diffstat (limited to 'source/gameengine/Ketsji/KX_SteeringActuator.cpp')
| -rw-r--r-- | source/gameengine/Ketsji/KX_SteeringActuator.cpp | 630 |
1 files changed, 630 insertions, 0 deletions
diff --git a/source/gameengine/Ketsji/KX_SteeringActuator.cpp b/source/gameengine/Ketsji/KX_SteeringActuator.cpp new file mode 100644 index 00000000000..a0a2e148c1e --- /dev/null +++ b/source/gameengine/Ketsji/KX_SteeringActuator.cpp @@ -0,0 +1,630 @@ +/** +* Add steering behaviors +* +* $Id$ +* +* ***** BEGIN GPL LICENSE BLOCK ***** +* +* This program is free software; you can redistribute it and/or +* modify it under the terms of the GNU General Public License +* as published by the Free Software Foundation; either version 2 +* of the License, or (at your option) any later version. The Blender +* Foundation also sells licenses for use in proprietary software under +* the Blender License. See http://www.blender.org/BL/ for information +* about this. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software Foundation, +* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +* +* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. +* All rights reserved. +* +* The Original Code is: all of this file. +* +* Contributor(s): none yet. +* +* ***** END GPL LICENSE BLOCK ***** +*/ + +#include "BLI_math.h" +#include "KX_SteeringActuator.h" +#include "KX_GameObject.h" +#include "KX_NavMeshObject.h" +#include "KX_ObstacleSimulation.h" +#include "KX_PythonInit.h" +#include "KX_PyMath.h" +#include "Recast.h" + +/* ------------------------------------------------------------------------- */ +/* Native functions */ +/* ------------------------------------------------------------------------- */ + +KX_SteeringActuator::KX_SteeringActuator(SCA_IObject *gameobj, + int mode, + KX_GameObject *target, + KX_GameObject *navmesh, + float distance, + float velocity, + float acceleration, + float turnspeed, + bool isSelfTerminated, + int pathUpdatePeriod, + KX_ObstacleSimulation* simulation, + short facingmode, + bool normalup, + bool enableVisualization) : + SCA_IActuator(gameobj, KX_ACT_STEERING), + m_mode(mode), + m_target(target), + m_distance(distance), + m_velocity(velocity), + m_acceleration(acceleration), + m_turnspeed(turnspeed), + m_isSelfTerminated(isSelfTerminated), + m_pathUpdatePeriod(pathUpdatePeriod), + m_updateTime(0), + m_isActive(false), + m_simulation(simulation), + m_enableVisualization(enableVisualization), + m_facingMode(facingmode), + m_normalUp(normalup), + m_obstacle(NULL), + m_pathLen(0), + m_wayPointIdx(-1), + m_steerVec(MT_Vector3(0, 0, 0)) +{ + m_navmesh = static_cast<KX_NavMeshObject*>(navmesh); + if (m_navmesh) + m_navmesh->RegisterActuator(this); + if (m_target) + m_target->RegisterActuator(this); + + if (m_simulation) + m_obstacle = m_simulation->GetObstacle((KX_GameObject*)gameobj); + KX_GameObject* parent = ((KX_GameObject*)gameobj)->GetParent(); + if (m_facingMode>0 && parent) + { + m_parentlocalmat = parent->GetSGNode()->GetLocalOrientation(); + } + else + m_parentlocalmat.setIdentity(); +} + +KX_SteeringActuator::~KX_SteeringActuator() +{ + if (m_navmesh) + m_navmesh->UnregisterActuator(this); + if (m_target) + m_target->UnregisterActuator(this); +} + +CValue* KX_SteeringActuator::GetReplica() +{ + KX_SteeringActuator* replica = new KX_SteeringActuator(*this); + // replication just copy the m_base pointer => common random generator + replica->ProcessReplica(); + return replica; +} + +void KX_SteeringActuator::ProcessReplica() +{ + if (m_target) + m_target->RegisterActuator(this); + if (m_navmesh) + m_navmesh->RegisterActuator(this); + SCA_IActuator::ProcessReplica(); +} + + +bool KX_SteeringActuator::UnlinkObject(SCA_IObject* clientobj) +{ + if (clientobj == m_target) + { + m_target = NULL; + return true; + } + else if (clientobj == m_navmesh) + { + m_navmesh = NULL; + return true; + } + return false; +} + +void KX_SteeringActuator::Relink(CTR_Map<CTR_HashedPtr, void*> *obj_map) +{ + void **h_obj = (*obj_map)[m_target]; + if (h_obj) { + if (m_target) + m_target->UnregisterActuator(this); + m_target = (KX_GameObject*)(*h_obj); + m_target->RegisterActuator(this); + } + + h_obj = (*obj_map)[m_navmesh]; + if (h_obj) { + if (m_navmesh) + m_navmesh->UnregisterActuator(this); + m_navmesh = (KX_NavMeshObject*)(*h_obj); + m_navmesh->RegisterActuator(this); + } +} + +bool KX_SteeringActuator::Update(double curtime, bool frame) +{ + if (frame) + { + double delta = curtime - m_updateTime; + m_updateTime = curtime; + + if (m_posevent && !m_isActive) + { + delta = 0; + m_pathUpdateTime = -1; + m_updateTime = curtime; + m_isActive = true; + } + bool bNegativeEvent = IsNegativeEvent(); + if (bNegativeEvent) + m_isActive = false; + + RemoveAllEvents(); + + if (!delta) + return true; + + if (bNegativeEvent || !m_target) + return false; // do nothing on negative events + + KX_GameObject *obj = (KX_GameObject*) GetParent(); + const MT_Point3& mypos = obj->NodeGetWorldPosition(); + const MT_Point3& targpos = m_target->NodeGetWorldPosition(); + MT_Vector3 vectotarg = targpos - mypos; + MT_Vector3 vectotarg2d = vectotarg; + vectotarg2d.z() = 0; + m_steerVec = MT_Vector3(0, 0, 0); + bool apply_steerforce = false; + bool terminate = true; + + switch (m_mode) { + case KX_STEERING_SEEK: + if (vectotarg2d.length2()>m_distance*m_distance) + { + terminate = false; + m_steerVec = vectotarg; + m_steerVec.normalize(); + apply_steerforce = true; + } + break; + case KX_STEERING_FLEE: + if (vectotarg2d.length2()<m_distance*m_distance) + { + terminate = false; + m_steerVec = -vectotarg; + m_steerVec.normalize(); + apply_steerforce = true; + } + break; + case KX_STEERING_PATHFOLLOWING: + if (m_navmesh && vectotarg.length2()>m_distance*m_distance) + { + terminate = false; + + static const MT_Scalar WAYPOINT_RADIUS(0.25); + + if (m_pathUpdateTime<0 || (m_pathUpdatePeriod>=0 && + curtime - m_pathUpdateTime>((double)m_pathUpdatePeriod/1000))) + { + m_pathUpdateTime = curtime; + m_pathLen = m_navmesh->FindPath(mypos, targpos, m_path, MAX_PATH_LENGTH); + m_wayPointIdx = m_pathLen > 1 ? 1 : -1; + } + + if (m_wayPointIdx>0) + { + MT_Vector3 waypoint(&m_path[3*m_wayPointIdx]); + if ((waypoint-mypos).length2()<WAYPOINT_RADIUS*WAYPOINT_RADIUS) + { + m_wayPointIdx++; + if (m_wayPointIdx>=m_pathLen) + { + m_wayPointIdx = -1; + terminate = true; + } + else + waypoint.setValue(&m_path[3*m_wayPointIdx]); + } + + m_steerVec = waypoint - mypos; + apply_steerforce = true; + + + if (m_enableVisualization) + { + //debug draw + static const MT_Vector3 PATH_COLOR(1,0,0); + m_navmesh->DrawPath(m_path, m_pathLen, PATH_COLOR); + } + } + + } + break; + } + + if (apply_steerforce) + { + bool isdyna = obj->IsDynamic(); + if (isdyna) + m_steerVec.z() = 0; + if (!m_steerVec.fuzzyZero()) + m_steerVec.normalize(); + MT_Vector3 newvel = m_velocity*m_steerVec; + + //adjust velocity to avoid obstacles + if (m_simulation && m_obstacle /*&& !newvel.fuzzyZero()*/) + { + if (m_enableVisualization) + KX_RasterizerDrawDebugLine(mypos, mypos + newvel, MT_Vector3(1.,0.,0.)); + m_simulation->AdjustObstacleVelocity(m_obstacle, m_mode!=KX_STEERING_PATHFOLLOWING ? m_navmesh : NULL, + newvel, m_acceleration*delta, m_turnspeed/180.0f*M_PI*delta); + if (m_enableVisualization) + KX_RasterizerDrawDebugLine(mypos, mypos + newvel, MT_Vector3(0.,1.,0.)); + } + + HandleActorFace(newvel); + if (isdyna) + { + //temporary solution: set 2D steering velocity directly to obj + //correct way is to apply physical force + MT_Vector3 curvel = obj->GetLinearVelocity(); + newvel.z() = curvel.z(); + obj->setLinearVelocity(newvel, false); + } + else + { + MT_Vector3 movement = delta*newvel; + obj->ApplyMovement(movement, false); + } + } + else + { + if (m_simulation && m_obstacle) + { + m_obstacle->dvel[0] = 0.f; + m_obstacle->dvel[1] = 0.f; + } + + } + + if (terminate && m_isSelfTerminated) + return false; + } + + return true; +} + +const MT_Vector3& KX_SteeringActuator::GetSteeringVec() +{ + static MT_Vector3 ZERO_VECTOR(0, 0, 0); + if (m_isActive) + return m_steerVec; + else + return ZERO_VECTOR; +} + +inline float vdot2(const float* a, const float* b) +{ + return a[0]*b[0] + a[2]*b[2]; +} +static bool barDistSqPointToTri(const float* p, const float* a, const float* b, const float* c) +{ + float v0[3], v1[3], v2[3]; + vsub(v0, c,a); + vsub(v1, b,a); + vsub(v2, p,a); + + const float dot00 = vdot2(v0, v0); + const float dot01 = vdot2(v0, v1); + const float dot02 = vdot2(v0, v2); + const float dot11 = vdot2(v1, v1); + const float dot12 = vdot2(v1, v2); + + // Compute barycentric coordinates + float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); + float u = (dot11 * dot02 - dot01 * dot12) * invDenom; + float v = (dot00 * dot12 - dot01 * dot02) * invDenom; + + float ud = u<0.f ? -u : (u>1.f ? u-1.f : 0.f); + float vd = v<0.f ? -v : (v>1.f ? v-1.f : 0.f); + return ud*ud+vd*vd ; +} + +inline void flipAxes(float* vec) +{ + std::swap(vec[1],vec[2]); +} + +static bool getNavmeshNormal(dtStatNavMesh* navmesh, const MT_Vector3& pos, MT_Vector3& normal) +{ + static const float polyPickExt[3] = {2, 4, 2}; + float spos[3]; + pos.getValue(spos); + flipAxes(spos); + dtStatPolyRef sPolyRef = navmesh->findNearestPoly(spos, polyPickExt); + if (sPolyRef == 0) + return false; + const dtStatPoly* p = navmesh->getPoly(sPolyRef-1); + const dtStatPolyDetail* pd = navmesh->getPolyDetail(sPolyRef-1); + + float distMin = FLT_MAX; + int idxMin = -1; + for (int i = 0; i < pd->ntris; ++i) + { + const unsigned char* t = navmesh->getDetailTri(pd->tbase+i); + const float* v[3]; + for (int j = 0; j < 3; ++j) + { + if (t[j] < p->nv) + v[j] = navmesh->getVertex(p->v[t[j]]); + else + v[j] = navmesh->getDetailVertex(pd->vbase+(t[j]-p->nv)); + } + float dist = barDistSqPointToTri(spos, v[0], v[1], v[2]); + if (dist<distMin) + { + distMin = dist; + idxMin = i; + } + } + + if (idxMin>=0) + { + const unsigned char* t = navmesh->getDetailTri(pd->tbase+idxMin); + const float* v[3]; + for (int j = 0; j < 3; ++j) + { + if (t[j] < p->nv) + v[j] = navmesh->getVertex(p->v[t[j]]); + else + v[j] = navmesh->getDetailVertex(pd->vbase+(t[j]-p->nv)); + } + MT_Vector3 tri[3]; + for (size_t j=0; j<3; j++) + tri[j].setValue(v[j][0],v[j][2],v[j][1]); + MT_Vector3 a,b; + a = tri[1]-tri[0]; + b = tri[2]-tri[0]; + normal = b.cross(a).safe_normalized(); + return true; + } + + return false; +} + +void KX_SteeringActuator::HandleActorFace(MT_Vector3& velocity) +{ + if (m_facingMode==0 && (!m_navmesh || !m_normalUp)) + return; + KX_GameObject* curobj = (KX_GameObject*) GetParent(); + MT_Vector3 dir = m_facingMode==0 ? curobj->NodeGetLocalOrientation().getColumn(1) : velocity; + if (dir.fuzzyZero()) + return; + dir.normalize(); + MT_Vector3 up(0,0,1); + MT_Vector3 left; + MT_Matrix3x3 mat; + + if (m_navmesh && m_normalUp) + { + dtStatNavMesh* navmesh = m_navmesh->GetNavMesh(); + MT_Vector3 normal; + MT_Vector3 trpos = m_navmesh->TransformToLocalCoords(curobj->NodeGetWorldPosition()); + if (getNavmeshNormal(navmesh, trpos, normal)) + { + + left = (dir.cross(up)).safe_normalized(); + dir = (-left.cross(normal)).safe_normalized(); + up = normal; + } + } + + switch (m_facingMode) + { + case 1: // TRACK X + { + left = dir.safe_normalized(); + dir = -(left.cross(up)).safe_normalized(); + break; + }; + case 2: // TRACK Y + { + left = (dir.cross(up)).safe_normalized(); + break; + } + + case 3: // track Z + { + left = up.safe_normalized(); + up = dir.safe_normalized(); + dir = left; + left = (dir.cross(up)).safe_normalized(); + break; + } + + case 4: // TRACK -X + { + left = -dir.safe_normalized(); + dir = -(left.cross(up)).safe_normalized(); + break; + }; + case 5: // TRACK -Y + { + left = (-dir.cross(up)).safe_normalized(); + dir = -dir; + break; + } + case 6: // track -Z + { + left = up.safe_normalized(); + up = -dir.safe_normalized(); + dir = left; + left = (dir.cross(up)).safe_normalized(); + break; + } + } + + mat.setValue ( + left[0], dir[0],up[0], + left[1], dir[1],up[1], + left[2], dir[2],up[2] + ); + + + + KX_GameObject* parentObject = curobj->GetParent(); + if(parentObject) + { + MT_Point3 localpos; + localpos = curobj->GetSGNode()->GetLocalPosition(); + MT_Matrix3x3 parentmatinv; + parentmatinv = parentObject->NodeGetWorldOrientation ().inverse (); + mat = parentmatinv * mat; + mat = m_parentlocalmat * mat; + curobj->NodeSetLocalOrientation(mat); + curobj->NodeSetLocalPosition(localpos); + } + else + { + curobj->NodeSetLocalOrientation(mat); + } + +} + +#ifndef DISABLE_PYTHON + +/* ------------------------------------------------------------------------- */ +/* Python functions */ +/* ------------------------------------------------------------------------- */ + +/* Integration hooks ------------------------------------------------------- */ +PyTypeObject KX_SteeringActuator::Type = { + PyVarObject_HEAD_INIT(NULL, 0) + "KX_SteeringActuator", + sizeof(PyObjectPlus_Proxy), + 0, + py_base_dealloc, + 0, + 0, + 0, + 0, + py_base_repr, + 0,0,0,0,0,0,0,0,0, + Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE, + 0,0,0,0,0,0,0, + Methods, + 0, + 0, + &SCA_IActuator::Type, + 0,0,0,0,0,0, + py_base_new +}; + +PyMethodDef KX_SteeringActuator::Methods[] = { + {NULL,NULL} //Sentinel +}; + +PyAttributeDef KX_SteeringActuator::Attributes[] = { + KX_PYATTRIBUTE_INT_RW("behaviour", KX_STEERING_NODEF+1, KX_STEERING_MAX-1, true, KX_SteeringActuator, m_mode), + KX_PYATTRIBUTE_RW_FUNCTION("target", KX_SteeringActuator, pyattr_get_target, pyattr_set_target), + KX_PYATTRIBUTE_RW_FUNCTION("navmesh", KX_SteeringActuator, pyattr_get_navmesh, pyattr_set_navmesh), + KX_PYATTRIBUTE_FLOAT_RW("distance", 0.0f, 1000.0f, KX_SteeringActuator, m_distance), + KX_PYATTRIBUTE_FLOAT_RW("velocity", 0.0f, 1000.0f, KX_SteeringActuator, m_velocity), + KX_PYATTRIBUTE_FLOAT_RW("acceleration", 0.0f, 1000.0f, KX_SteeringActuator, m_acceleration), + KX_PYATTRIBUTE_FLOAT_RW("turnspeed", 0.0f, 720.0f, KX_SteeringActuator, m_turnspeed), + KX_PYATTRIBUTE_BOOL_RW("selfterminated", KX_SteeringActuator, m_isSelfTerminated), + KX_PYATTRIBUTE_BOOL_RW("enableVisualization", KX_SteeringActuator, m_enableVisualization), + KX_PYATTRIBUTE_RO_FUNCTION("steeringVec", KX_SteeringActuator, pyattr_get_steeringVec), + KX_PYATTRIBUTE_SHORT_RW("facingMode", 0, 6, true, KX_SteeringActuator, m_facingMode), + KX_PYATTRIBUTE_INT_RW("pathUpdatePeriod", -1, 100000, true, KX_SteeringActuator, m_pathUpdatePeriod), + { NULL } //Sentinel +}; + +PyObject* KX_SteeringActuator::pyattr_get_target(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef) +{ + KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self); + if (!actuator->m_target) + Py_RETURN_NONE; + else + return actuator->m_target->GetProxy(); +} + +int KX_SteeringActuator::pyattr_set_target(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value) +{ + KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self); + KX_GameObject *gameobj; + + if (!ConvertPythonToGameObject(value, &gameobj, true, "actuator.object = value: KX_SteeringActuator")) + return PY_SET_ATTR_FAIL; // ConvertPythonToGameObject sets the error + + if (actuator->m_target != NULL) + actuator->m_target->UnregisterActuator(actuator); + + actuator->m_target = (KX_GameObject*) gameobj; + + if (actuator->m_target) + actuator->m_target->RegisterActuator(actuator); + + return PY_SET_ATTR_SUCCESS; +} + +PyObject* KX_SteeringActuator::pyattr_get_navmesh(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef) +{ + KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self); + if (!actuator->m_navmesh) + Py_RETURN_NONE; + else + return actuator->m_navmesh->GetProxy(); +} + +int KX_SteeringActuator::pyattr_set_navmesh(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value) +{ + KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self); + KX_GameObject *gameobj; + + if (!ConvertPythonToGameObject(value, &gameobj, true, "actuator.object = value: KX_SteeringActuator")) + return PY_SET_ATTR_FAIL; // ConvertPythonToGameObject sets the error + + if (!PyObject_TypeCheck(value, &KX_NavMeshObject::Type)) + { + PyErr_Format(PyExc_TypeError, "KX_NavMeshObject is expected"); + return PY_SET_ATTR_FAIL; + } + + if (actuator->m_navmesh != NULL) + actuator->m_navmesh->UnregisterActuator(actuator); + + actuator->m_navmesh = static_cast<KX_NavMeshObject*>(gameobj); + + if (actuator->m_navmesh) + actuator->m_navmesh->RegisterActuator(actuator); + + return PY_SET_ATTR_SUCCESS; +} + +PyObject* KX_SteeringActuator::pyattr_get_steeringVec(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef) +{ + KX_SteeringActuator* actuator = static_cast<KX_SteeringActuator*>(self); + const MT_Vector3& steeringVec = actuator->GetSteeringVec(); + return PyObjectFrom(steeringVec); +} + +#endif // DISABLE_PYTHON + +/* eof */ + |