void KX_ObstacleSimulation::DrawObstacles()
{
if (!m_enableVisualization)
return;
static const MT_Vector3 bluecolor(0,0,1);
static const MT_Vector3 normal(0.,0.,1.);
static const int SECTORS_NUM = 32;
for (size_t i=0; i<m_obstacles.size(); i++)
{
if (m_obstacles[i]->m_shape==KX_OBSTACLE_SEGMENT)
{
MT_Point3 p1 = m_obstacles[i]->m_pos;
MT_Point3 p2 = m_obstacles[i]->m_pos2;
//apply world transform
if (m_obstacles[i]->m_type == KX_OBSTACLE_NAV_MESH)
{
KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(m_obstacles[i]->m_gameObj);
p1 = navmeshobj->TransformToWorldCoords(p1);
p2 = navmeshobj->TransformToWorldCoords(p2);
}
KX_RasterizerDrawDebugLine(p1, p2, bluecolor);
}
else if (m_obstacles[i]->m_shape==KX_OBSTACLE_CIRCLE)
{
KX_RasterizerDrawDebugCircle(m_obstacles[i]->m_pos, m_obstacles[i]->m_rad, bluecolor,
normal, SECTORS_NUM);
}
}
}
void KX_NavMeshObject::DrawPath(const float *path, int pathLen, const MT_Vector3& color)
{
MT_Vector3 a,b;
for (int i=0; i<pathLen-1; i++)
{
a.setValue(&path[3*i]);
b.setValue(&path[3*(i+1)]);
KX_RasterizerDrawDebugLine(a, b, color);
}
}
virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)
{
if (m_debugMode >0)
{
MT_Vector3 kxfrom(from[0],from[1],from[2]);
MT_Vector3 kxto(to[0],to[1],to[2]);
MT_Vector3 kxcolor(color[0],color[1],color[2]);
KX_RasterizerDrawDebugLine(kxfrom,kxto,kxcolor);
}
}
void KX_NavMeshObject::DrawNavMesh(NavMeshRenderMode renderMode)
{
if (!m_navMesh)
return;
MT_Vector3 color(0.f, 0.f, 0.f);
switch (renderMode)
{
case RM_POLYS :
case RM_WALLS :
for (int pi=0; pi<m_navMesh->getPolyCount(); pi++)
{
const dtStatPoly* poly = m_navMesh->getPoly(pi);
for (int i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j = i++)
{
if (poly->n[j] && renderMode==RM_WALLS)
continue;
const float* vif = m_navMesh->getVertex(poly->v[i]);
const float* vjf = m_navMesh->getVertex(poly->v[j]);
MT_Point3 vi(vif[0], vif[2], vif[1]);
MT_Point3 vj(vjf[0], vjf[2], vjf[1]);
vi = TransformToWorldCoords(vi);
vj = TransformToWorldCoords(vj);
KX_RasterizerDrawDebugLine(vi, vj, color);
}
}
break;
case RM_TRIS :
for (int i = 0; i < m_navMesh->getPolyDetailCount(); ++i)
{
const dtStatPoly* p = m_navMesh->getPoly(i);
const dtStatPolyDetail* pd = m_navMesh->getPolyDetail(i);
for (int j = 0; j < pd->ntris; ++j)
{
const unsigned char* t = m_navMesh->getDetailTri(pd->tbase+j);
MT_Point3 tri[3];
for (int k = 0; k < 3; ++k)
{
const float* v;
if (t[k] < p->nv)
v = m_navMesh->getVertex(p->v[t[k]]);
else
v = m_navMesh->getDetailVertex(pd->vbase+(t[k]-p->nv));
float pos[3];
rcVcopy(pos, v);
flipAxes(pos);
tri[k].setValue(pos);
}
for (int k=0; k<3; k++)
tri[k] = TransformToWorldCoords(tri[k]);
for (int k=0; k<3; k++)
KX_RasterizerDrawDebugLine(tri[k], tri[(k+1)%3], color);
}
}
break;
default:
/* pass */
break;
}
}
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.0;
m_pathUpdateTime = -1.0;
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_Vector3& mypos = obj->NodeGetWorldPosition();
const MT_Vector3& targpos = m_target->NodeGetWorldPosition();
MT_Vector3 vectotarg = targpos - mypos;
MT_Vector3 vectotarg2d = vectotarg;
vectotarg2d.z() = 0.0f;
m_steerVec = MT_Vector3(0.0f, 0.0f, 0.0f);
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.25f);
if (m_pathUpdateTime<0 || (m_pathUpdatePeriod>=0 &&
curtime - m_pathUpdateTime>((double)m_pathUpdatePeriod/1000.0)))
{
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_Vector4 PATH_COLOR(1.0f, 0.0f, 0.0f, 1.0f);
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_Vector4(1.0f, 0.0f, 0.0f, 1.0f));
m_simulation->AdjustObstacleVelocity(m_obstacle, m_mode!=KX_STEERING_PATHFOLLOWING ? m_navmesh : NULL,
newvel, m_acceleration*(float)delta, m_turnspeed/(180.0f*(float)(M_PI*delta)));
if (m_enableVisualization)
KX_RasterizerDrawDebugLine(mypos, mypos + newvel, MT_Vector4(0.0f, 1.0f, 0.0f, 1.0f));
}
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();
if (m_lockzvel)
newvel.z() = 0.0f;
else
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;
}