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; }