bool AIRobot::isCloseEnough()
{
mWorldTarget = GridToWorld(mGridTarget);
Vector3 realPos(mNode->GetLocalTranslation());
float delX = (realPos.x - mWorldTarget.x);
float delZ = (realPos.z - mWorldTarget.y);
return ((delX*delX + delZ*delZ) <= mDistMargin);
}
void Level::GridToWorld(const std::vector<PathFinder::SimpleNode>& gridPath, std::vector<DOUBLE2>& worldPath)
{
worldPath.clear();
worldPath.reserve(gridPath.size());
for (std::vector<PathFinder::SimpleNode>::const_iterator it = gridPath.cbegin(); it != gridPath.cend(); ++it)
{
worldPath.push_back(GridToWorld(*it));
}
}
/**
* @function NodePathToWorkspacePath
*/
std::vector< std::vector<Eigen::VectorXd> > LJM2::NodePathToWorkspacePath( std::vector< std::vector<Eigen::Vector3i> > _nodePath ) {
std::vector< std::vector<Eigen::VectorXd> > workspacePath;
for( size_t i = 0; i < _nodePath.size(); ++i ) {
std::vector<Eigen::VectorXd> path;
for( size_t j = 0; j < _nodePath[i].size(); ++j ) {
Eigen::VectorXd temp(3);
GridToWorld( _nodePath[i][j](0), _nodePath[i][j](1), _nodePath[i][j](2), temp(0), temp(1), temp(2) );
path.push_back( temp );
}
workspacePath.push_back( path );
}
return workspacePath;
}
void AIRobot::MoveToTarget(float elapsedTime)
{
// We assume here that mWorldTarget is OK
Vector3 curPos = mNode->GetLocalTranslation();
mWorldTarget = GridToWorld(mGridTarget);
Vector2f dir(mWorldTarget.x - curPos.x, mWorldTarget.y - curPos.z);
Vector3 v(dir.x,0,dir.y);
v.Normalize();
v *= mMoveSpeed;
v *= elapsedTime;
v.y = 0;
mNode->SetTranslation(curPos+v);
}
AIRobot::AIRobot(const VCNNodeID& nodeId, std::vector<std::vector<dir>> maze, Vector2i pos)
: mNodeId(nodeId),
mGridPos(pos),
mGridTarget(pos),
mMaze(maze),
mMoveSpeed(3.0f),
//mOnWorldTarget(true),
mFieldViewAngle(10.0f),
mDeltaAngle(22.5f),
mDistMargin(5.0f)
{
const VCNNodeID mMazeNodeId = VCNNodeCore::GetInstance()->GetNodeByName(VCNTXT("Cylinder002"));
VCNNode* mMazeNode = VCNNodeCore::GetInstance()->GetNode(mMazeNodeId);
mNode = VCNNodeCore::GetInstance()->GetNode(mNodeId);
mWorldTarget = GridToWorld(mGridTarget);
mNode->SetTranslation(Vector3(mWorldTarget.x,0,mWorldTarget.y));
mMazeCenter = mMazeNode->GetWorldTranslation();
// Initialization of the random seed
srand(time(NULL));
}
