diff options
author | Nick Samarin <nicks1987@bigmir.net> | 2010-08-15 21:40:57 +0400 |
---|---|---|
committer | Nick Samarin <nicks1987@bigmir.net> | 2010-08-15 21:40:57 +0400 |
commit | b5f4144d251989be378659858ed38aaf572ff4ec (patch) | |
tree | 5a4bfc9b5cf5513fb75d00608c76a78efdb4020b /source/gameengine/Ketsji | |
parent | e6412cd5c6f267c2fbefb4fe42e4f8e26f1bfcd4 (diff) |
added option to set "up" vector of the actor orientation using navmesh normal (steering actuator)
Diffstat (limited to 'source/gameengine/Ketsji')
-rw-r--r-- | source/gameengine/Ketsji/KX_SteeringActuator.cpp | 119 | ||||
-rw-r--r-- | source/gameengine/Ketsji/KX_SteeringActuator.h | 2 |
2 files changed, 116 insertions, 5 deletions
diff --git a/source/gameengine/Ketsji/KX_SteeringActuator.cpp b/source/gameengine/Ketsji/KX_SteeringActuator.cpp index b91faf2646f..31dcd00e61f 100644 --- a/source/gameengine/Ketsji/KX_SteeringActuator.cpp +++ b/source/gameengine/Ketsji/KX_SteeringActuator.cpp @@ -39,7 +39,7 @@ #include "KX_ObstacleSimulation.h" #include "KX_PythonInit.h" #include "KX_PyMath.h" - +#include "Recast.h" /* ------------------------------------------------------------------------- */ /* Native functions */ @@ -57,6 +57,7 @@ KX_SteeringActuator::KX_SteeringActuator(SCA_IObject *gameobj, int pathUpdatePeriod, KX_ObstacleSimulation* simulation, short facingmode, + bool normalup, bool enableVisualization) : SCA_IActuator(gameobj, KX_ACT_STEERING), m_mode(mode), @@ -72,6 +73,7 @@ KX_SteeringActuator::KX_SteeringActuator(SCA_IObject *gameobj, m_simulation(simulation), m_enableVisualization(enableVisualization), m_facingMode(facingmode), + m_normalUp(normalup), m_obstacle(NULL), m_pathLen(0), m_wayPointIdx(-1), @@ -309,23 +311,129 @@ bool KX_SteeringActuator::Update(double curtime, bool frame) const MT_Vector3& KX_SteeringActuator::GetSteeringVec() { + static MT_Vector3 ZERO_VECTOR(0, 0, 0); if (m_isActive) return m_steerVec; else - return MT_Vector3(0, 0, 0); + 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) + if (m_facingMode==0 && (!m_navmesh || !m_normalUp)) return; - MT_Vector3 dir = velocity; + 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 @@ -370,13 +478,14 @@ void KX_SteeringActuator::HandleActorFace(MT_Vector3& velocity) break; } } + mat.setValue ( left[0], dir[0],up[0], left[1], dir[1],up[1], left[2], dir[2],up[2] ); - KX_GameObject* curobj = (KX_GameObject*) GetParent(); + KX_GameObject* parentObject = curobj->GetParent(); if(parentObject) diff --git a/source/gameengine/Ketsji/KX_SteeringActuator.h b/source/gameengine/Ketsji/KX_SteeringActuator.h index 78fb29f4124..2b2dc9ae923 100644 --- a/source/gameengine/Ketsji/KX_SteeringActuator.h +++ b/source/gameengine/Ketsji/KX_SteeringActuator.h @@ -66,6 +66,7 @@ class KX_SteeringActuator : public SCA_IActuator bool m_isSelfTerminated; bool m_enableVisualization; short m_facingMode; + bool m_normalUp; float m_path[MAX_PATH_LENGTH*3]; int m_pathLen; int m_pathUpdatePeriod; @@ -96,6 +97,7 @@ public: int pathUpdatePeriod, KX_ObstacleSimulation* simulation, short facingmode, + bool normalup, bool enableVisualization); virtual ~KX_SteeringActuator(); virtual bool Update(double curtime, bool frame); |