int KX_NavMeshObject::FindPath(const MT_Point3& from, const MT_Point3& to, float* path, int maxPathLen)
{
if (!m_navMesh)
return 0;
MT_Point3 localfrom = TransformToLocalCoords(from);
MT_Point3 localto = TransformToLocalCoords(to);
float spos[3], epos[3];
localfrom.getValue(spos); flipAxes(spos);
localto.getValue(epos); flipAxes(epos);
dtStatPolyRef sPolyRef = m_navMesh->findNearestPoly(spos, polyPickExt);
dtStatPolyRef ePolyRef = m_navMesh->findNearestPoly(epos, polyPickExt);
int pathLen = 0;
if (sPolyRef && ePolyRef)
{
dtStatPolyRef* polys = new dtStatPolyRef[maxPathLen];
int npolys;
npolys = m_navMesh->findPath(sPolyRef, ePolyRef, spos, epos, polys, maxPathLen);
if (npolys)
{
pathLen = m_navMesh->findStraightPath(spos, epos, polys, npolys, path, maxPathLen);
for (int i=0; i<pathLen; i++)
{
flipAxes(&path[i*3]);
MT_Point3 waypoint(&path[i*3]);
waypoint = TransformToWorldCoords(waypoint);
waypoint.getValue(&path[i*3]);
}
}
delete[] polys;
}
return pathLen;
}
//-----------------------------------------------------------------------------
//! Saves a waypoint using a waypoint dialog
//-----------------------------------------------------------------------------
void tSonarPlayer::PromptSaveWaypoint()
{
if( m_pSonarPlayerWidget->CursorMode() )
{
if( !UDB::GetUDBWpRtDB()->IsLoaded() )
{
tWpRtUtils::ShowWaypointDbNotLoadedPrompt();
return;
}
tSonarColumnDigitalData Data;
float CursorDepth, CursorTemperature, DisplayDepth;
bool TempValid;
unsigned long TimeOffset;
qint64 sequenceNum;
int DebugFrequency_Hz;
int rangeCellValue;
if( m_pSonarPlayerWidget->GetCursorData( Data, DisplayDepth, CursorDepth, CursorTemperature, TempValid, TimeOffset, DebugFrequency_Hz, rangeCellValue, sequenceNum ) && Data.PositionValid() )
{
tWaypoint waypoint(
QUuid::createUuid(),
UDB::GetUDBWpRtDB()->GenerateUid(),
UDB::GetUDBWpRtDB()->GenerateWaypointName(),
Data.RPosition(),
tWaypointSettings::Instance()->DisplayConfig() );
if(m_pSonarPlayerWidget->IsVertical())
{
if(Data.DepthValid())
waypoint.SetDepth( Data.m_DepthBelowTransducer );
else
waypoint.SetDepth(0);
}
else
{
waypoint.SetDepth( fabs( CursorDepth ) );
}
tWaypointDialog* pWaypointDialog = tUiFactory::Instance()->WaypointDialog( &waypoint, tWaypointDialog::NEW, this, tWaypointDialog::CURSOR );
int ret = pWaypointDialog->exec();
if( ret == QDialog::Accepted )
{
m_pSonarPlayerWidget->WaypointCreated( sequenceNum, waypoint.GetUuid(), CursorDepth );
}
delete pWaypointDialog;
}
else
{
// Popup an error box saying position is not valid...
tMessageBox::Information( this, QString(), tr( "Position not valid at this location", "Warning message" ), tMessageBox::CLOSE );
}
}
else
{
Assert(false);
}
}
void RoutingLayerPrivate::storeDragPosition( const QPoint &pos )
{
m_dragStopOver = pos;
m_dragStopOverRightIndex = -1;
qreal lon( 0.0 ), lat( 0.0 );
if ( m_routeRequest && !pos.isNull()
&& m_marbleWidget->geoCoordinates( pos.x(), pos.y(), lon, lat, GeoDataCoordinates::Radian ) ) {
GeoDataCoordinates waypoint( lon, lat );
m_dragStopOverRightIndex = m_routingModel->rightNeighbor( waypoint, m_routeRequest );
}
}
//load initial path
// file has following form (like a bvh)
// HIERARCHY
// Ordered list of all joints for which there are constraints
// MOTION
// Frames: 16
// Frame Time: 1
// "X Y Z Rx Ry Rz J1 J2... Jn" one line per state
std::vector<std::string> InitTrajectoryFromFile(std::vector<Eigen::VectorXd>& waypoints, const std::string& filepath)
{
std::vector<std::string> res;
bool hierarchy = false;
bool motion = false;
std::ifstream myfile (filepath.c_str());
if (myfile.is_open())
{
std::string line;
while (myfile.good())
{
getline(myfile, line);
if(line.find("HIERARCHY") != std::string::npos)
{
hierarchy = true;
}
else if(line.find("MOTION") != std::string::npos)
{
hierarchy = false;
}
else if(line.find("Frame Time") != std::string::npos)
{
motion = true;
}
else if(!line.empty())
{
if(hierarchy)
{
res.push_back(line);
}
else if(motion)
{
Eigen::VectorXd waypoint(res.size());
char *endptr;
int h = 0;
waypoint[h++] = strtod(line.c_str(), &endptr);
for(; h< res.size(); ++h)
{
waypoint[h] = strtod(endptr, &endptr);
}
waypoints.push_back(waypoint);
}
}
}
myfile.close();
}
else
{
std::cout << "can not find initial path file" << filepath << std::endl;
}
return res;
}
void passive_red_line_correct() {
// only correct to red line if not backing up
if (abs(y - RENDEZVOUS_Y) < GRID_WIDTH*0.5 && layers[active_layer].speed > 0) {
digitalWrite(bottom_led, HIGH);
if (on_line(CENTER)) cycles_on_red_line = 0;
else if (on_line(RED) && !on_line(CENTER)) {
++cycles_on_red_line;
// navigate trying to get back to red line and x is far enough forward
if (seeking_red_line && RENDEZVOUS_X - x < 3*RENDEZVOUS_CLOSE) {
waypoint(LAYER_NAV);
}
}
else if (!on_line(RED)) {
// not false alarm
if (cycles_on_red_line >= CYCLES_CROSSING_LINE && current_distance() - last_correct_distance > DISTANCE_CENTER_TO_RED_ALLOWANCE) {
SERIAL_PRINT("RC");
SERIAL_PRINTLN(current_distance() - last_correct_distance);
// direction and signs are taken care of by sin and cos
float offset_y = DISTANCE_CENTER_TO_RED * sin(theta);
// avoid correcting when parallel to horizontal line
int offset_x = abs((int)x) % GRID_WIDTH;
if (abs(offset_y) > NEED_TO_HOPPER_CORRECT && (offset_x < INTERSECTION_TOO_CLOSE*0.5 || offset_x > GRID_WIDTH - INTERSECTION_TOO_CLOSE*0.5) && parallel_to_horizontal()) {
SERIAL_PRINTLN("-RC");
cycles_on_red_line = 0;
last_correct_distance = current_distance();
return;
}
y = RENDEZVOUS_Y;
// between [-180,0] left while going left
// x += DISTANCE_CENTER_TO_RED * cos(theta);
if (theta < 0) offset_y -= HALF_LINE_WIDTH;
else offset_y += HALF_LINE_WIDTH;
y += offset_y;
last_red_line_distance = current_distance();
last_correct_distance = last_red_line_distance;
if (side_of_board == SIDE_RIGHT) side_of_board = SIDE_LEFT;
else if (side_of_board == SIDE_LEFT) side_of_board = SIDE_RIGHT;
}
cycles_on_red_line = 0;
}
}
else {
digitalWrite(bottom_led, LOW);
cycles_on_red_line = 0;
}
}
/** @brief This function initialize waypoints while setting
* the next waypoints.
*/
std::vector<Waypoint> GameStatePlay::addWaypoints(std::vector<sf::Vector2f> path_points) {
std::vector<Waypoint> waypoints;
//Initialize waypoints
for (int i = 0; i < int(path_points.size()); ++i) {
Waypoint waypoint(path_points[i]);
waypoints.push_back(waypoint);
}
//Set next waypoints
for (int i = 0; i < int(waypoints.size()); ++i) {
if (i != waypoints.size() - 1) {
waypoints[i].next_waypoint = &waypoints[i+1];
}
if (i == waypoints.size() - 1){
waypoints[i].next_waypoint = NULL;
}
}
return waypoints;
}
bool npc_escortAI::SetNextWaypoint(uint32 pointId, bool setPosition, bool resetWaypointsOnFail)
{
if (!WaypointList.empty())
WaypointList.clear();
FillPointMovementListForCreature();
if (WaypointList.empty())
return false;
size_t const size = WaypointList.size();
Escort_Waypoint waypoint(0, 0, 0, 0, 0);
do
{
waypoint = WaypointList.front();
WaypointList.pop_front();
if (waypoint.id == pointId)
{
if (setPosition)
me->UpdatePosition(waypoint.x, waypoint.y, waypoint.z, me->GetOrientation());
CurrentWP = WaypointList.begin();
return true;
}
}
while (!WaypointList.empty());
// we failed.
// we reset the waypoints in the start; if we pulled any, reset it again
if (resetWaypointsOnFail && size != WaypointList.size())
{
if (!WaypointList.empty())
WaypointList.clear();
FillPointMovementListForCreature();
}
return false;
}
_export void * module_thread(void *arg) {
time_t run_start;
time(&run_start);
moveto(5132, 5165);
moveto(5133, 5145);
moveto(5133, 5121);
moveto(5131, 5095);
moveto(5148, 5090);
moveto(5149, 5067);
moveto(5160, 5053);
waypoint(0x65);
sleep(1);
extension("find_level_exit")->call("meph", "Durance of Hate Level 2", "Mephisto Down");
sleep(1);
d2gs_send(0x69, "");
time_t cur;
int runtime = (int) difftime(time(&cur), run_start);
char *s_runtime = string_format_time(runtime);
plugin_print("meph", "run took %s\n", s_runtime);
free(s_runtime);
pthread_exit(NULL);
}
bool
WaypointReaderCompeGPS::ParseLine(const TCHAR* line, const unsigned linenum,
Waypoints &waypoints)
{
/*
* G WGS 84
* U 1
* W IT05FC A 46.9121939503ºN 11.9605922700°E 27-MAR-62 00:00:00 566.000000 Ahornach Sand, Ahornach LP, GS und HG
* w Waypoint,0,-1.0,16777215,255,0,0,7,,0.0,
* W IT05FB A 46.9260440931ºN 11.9676733017°E 27-MAR-62 00:00:00 1425.000000 Ahornach Sand, Ahornach SP, GS und HG
* w Waypoint,0,-1.0,16777215,255,0,0,7,,0.0,
*
* W ShortName 31T 318570 4657569 27-MAR-62 00:00:00 0 some Comments
* W ShortName A 41.234234N 7.234424W 27-MAR-62 00:00:00 0 Comments
*/
// Skip projection and file encoding information
if (*line == _T('G') || *line == _T('B'))
return true;
// Check for format: UTM or LatLon
if (*line == _T('U') && _tcsstr(line, _T("U 0")) == line) {
is_utm = true;
return true;
}
// Skip non-waypoint lines
if (*line != _T('W'))
return true;
// Skip W indicator and whitespace
line++;
while (*line == _T(' '))
line++;
// Find next space delimiter, skip shortname
const TCHAR *name = line;
const TCHAR *space = _tcsstr(line, _T(" "));
if (space == NULL)
return false;
unsigned name_length = space - line;
if (name_length == 0)
return false;
line = space;
while (*line == _T(' '))
line++;
// Parse location
GeoPoint location;
if ((!is_utm && !ParseLocation(line, location)) ||
(is_utm && !ParseLocationUTM(line, location)))
return false;
// Skip whitespace
while (*line == _T(' '))
line++;
// Skip unused date field
line = _tcsstr(line, _T(" "));
if (line == NULL)
return false;
line++;
// Skip unused time field
line = _tcsstr(line, _T(" "));
if (line == NULL)
return false;
line++;
// Create new waypoint instance
Waypoint waypoint(location);
waypoint.file_num = file_num;
waypoint.original_id = 0;
waypoint.name.assign(name, name_length);
// Parse altitude
if (!ParseAltitude(line, waypoint.elevation) &&
!CheckAltitude(waypoint))
return false;
// Skip whitespace
while (*line == _T(' '))
line++;
// Parse waypoint name
waypoint.comment.assign(line);
waypoints.Append(std::move(waypoint));
return true;
}
vector<point> pathfind(City& c, int x1, int y1, vector<point> dests) {
vector<point> r;
vector<waypoint> pheap{ waypoint( -1, 1, x1, y1 ) };
vector<int> dist(c.getXSize() * c.getYSize());
int xsz = c.getXSize();
while (pheap.size() > 0) {
for (auto d : dests)
if (dist[d.first + d.second * xsz] != 0)
break;
pop_heap(pheap.begin(), pheap.end());
waypoint p = pheap.back();
pheap.pop_back();
int& d = dist[get<YCOORD>(p) * xsz + get<XCOORD>(p)];
if (d != 0) continue;
d = get<DIST>(p);
for (int i=0;i<4;++i) {
int x = get<XCOORD>(p) + offs[i].first;
int y = get<YCOORD>(p) + offs[i].second;
if (x >= 0 && x < c.getXSize() && y >= 0 && y < c.getYSize()) {
if (!c.tile(x,y).walkable()) continue;
int& d2 = dist[y * xsz + x];
if (d2 > d + 1 || d2 == 0) {
pheap.push_back(waypoint(-d-1, d + 1, x, y));
push_heap(pheap.begin(), pheap.end());
}
//cout << "A " << x << " " << y << " " << d2 << endl;
}
}
}
// for (int y = 0; y < c.getYSize(); ++y) {
// for (int x = 0; x < c.getXSize(); ++x)
// cout << setw(2) << dist[y*xsz + x] << " ";
// cout << endl;
// }
point* p = nullptr;
for (auto d : dests)
if (dist[d.first + d.second * xsz] != 0) {
p = &d;
break;
}
if (p == nullptr)
return r;
int d = dist[p->first + xsz*p->second];
r.reserve(d);
int x = p->first;
int y = p->second;
while (x != x1 || y != y1) {
for (int i=0;i<4;++i) {
if (c.check(x+offs[i].first, y+offs[i].second))
if (dist[(y + offs[i].second)*xsz + x + offs[i].first] == d - 1) {
d = d - 1;
r.push_back({x, y});
x = x + offs[i].first;
y = y + offs[i].second;
break;
}
}
}
return r;
}
vector<point> pathfind(City& c, int x1, int y1, int x2, int y2) {
vector<waypoint> pheap{ waypoint( -(abs(x1-x2) + abs(y1-y2) + 1), 1, x1, y1 ) };
vector<int> dist(c.getXSize() * c.getYSize());
int xsz = c.getXSize();
while (pheap.size() > 0 && dist[y2 * xsz + x2] == 0) {
pop_heap(pheap.begin(), pheap.end());
tuple<int,int,int,int> p = pheap.back();
pheap.pop_back();
// cout << "(" << pheap.size() << ") Popped: "
// << get<0>(p) << ","
// << get<1>(p) << ","
// << get<2>(p) << ","
// << get<3>(p) << endl;
int& d = dist[get<YCOORD>(p) * xsz + get<XCOORD>(p)];
if (d != 0) continue;
d = get<DIST>(p);
for (int i=0;i<4;++i) {
int x = get<XCOORD>(p) + offs[i].first;
int y = get<YCOORD>(p) + offs[i].second;
if (x >= 0 && x < c.getXSize() && y >= 0 && y < c.getYSize()) {
if (!c.tile(x,y).walkable()) continue;
int& d2 = dist[y * xsz + x];
if (d2 > d + 1 || d2 == 0) {
pheap.push_back(waypoint(-abs(x-x2) - abs(y-y2) - d - 1, d + 1, x, y));
push_heap(pheap.begin(), pheap.end());
}
//cout << "A " << x << " " << y << " " << d2 << endl;
}
}
}
// for (int y = 0; y < c.getYSize(); ++y) {
// for (int x = 0; x < c.getXSize(); ++x)
// cout << setw(2) << dist[y*xsz + x] << " ";
// cout << endl;
// }
int d = dist[y2 * xsz + x2];
vector<point> r;
r.reserve(d);
if (d != 0) {
int x = x2;
int y = y2;
while (x != x1 || y != y1) {
for (int i=0;i<4;++i) {
if (c.check(x+offs[i].first, y+offs[i].second))
if (dist[(y + offs[i].second)*xsz + x + offs[i].first] == d - 1) {
d = d - 1;
r.push_back({x, y});
x = x + offs[i].first;
y = y + offs[i].second;
break;
}
}
}
}
return r;
}
CreateWaypoint::CreateWaypoint(unsigned short i, const location& l, bool c):
CreationStep<waypoint>(i,waypoint(l),c) {}
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;
}
int main()
{
// push waypoints to a routes' map
waypoints.push_back(waypoint(-4.0, -3.0)); // 0
waypoints.push_back(waypoint(-3.0, -1.0)); // 1
waypoints.push_back(waypoint(1.0, -4.0)); // 2
waypoints.push_back(waypoint(3.0, -2.0)); // 3
waypoints.push_back(waypoint(-1.0, 1.0)); // 4
waypoints.push_back(waypoint(4.0, -3.0)); // 5
waypoints.push_back(waypoint(2.0, 1.0)); // 6
waypoints.push_back(waypoint(-2.0, 2.0)); // 7
waypoints.push_back(waypoint(5.0, -1.0)); // 8
waypoints.push_back(waypoint(6.0, 2.0)); // 9
waypoints.push_back(waypoint(-1.0, 4.0)); // 10
waypoints.push_back(waypoint(4.0, 4.0)); // 11
waypoints.push_back(waypoint(8.0, 3.0)); // 12
waypoints.push_back(waypoint(1.0, 6.0)); // 13
waypoints.push_back(waypoint(6.0, 5.0)); // 14
waypoints.push_back(waypoint(2.0, 7.0)); // 15
waypoints.push_back(waypoint(7.0, 7.0)); // 16
waypoints.push_back(waypoint(4.0, 7.0)); // 17
waypoints.push_back(waypoint(6.0, 9.0)); // 18
waypoints.push_back(waypoint(8.0, 9.0)); // 19
obstacles.push_back(obstacle(0.0,5.0,1.0)); // 0
obstacles.push_back(obstacle(3.0,3.0,2.0)); // 1
obstacles.push_back(obstacle(3.5,0.0,1.0)); // 2
obstacles.push_back(obstacle(8.0,7.0,2.5)); // 3
// define exponential traverser type and create an exemplar
typedef ExpTr<node_value,cost_value,expand,less<cost_value> > ExpTr_t;
ExpTr_t j;
cout << "in exp traverser forward search ...\n";
j = ExpTr_t(0);
// set a maximal lag length
j.getexpfunc().setMaxLagLen(5.0); // or 6.0
// add two defined handlers to enable calculation of an expansions number
j.set_handler_on_expand_root(OnExpandRoot);
j.set_handler_on_select_cursor(OnSelectCursor);
// launch A* algorithm (combination of a predefined heuristics with a best - first
// search that is provided by exponential traverser is A*!)
ForwardSearch(j, goal);
ShowPath(j);
// show founded path
cout << "number of expansions = " << num_expansions << '\n';
cout << "...out \n\n";
cout << "Search is completed.\n";
return 0;
}
//load initial path
// file has following form (like a bvh)
// HIERARCHY
// Ordered list of all joints for which there are constraints
// MOTION
// Frames: 16
// Frame Time: 1
// "X Y Z Rx Ry Rz J1 J2... Jn" one line per state
// "CONTACT EFFECTORS N" N is the number of effectors
// "hasContact X Y Z qx qy qz qw" hasContact 0 or 1, times number effectors one line per state
std::vector<std::string> InitTrajectoryFromFile(std::vector<Eigen::VectorXd>& waypoints, std::vector<Eigen::MatrixXd>& contactPoints, const std::string& filepath)
{
std::vector<std::string> res;
bool hierarchy = false;
bool motion = false;
bool contacts = false;
double offset [3] = {0,0,0};
int nbEffectors = 0;
std::ifstream myfile (filepath.c_str());
if (myfile.is_open())
{
std::string line;
while (myfile.good())
{
getline(myfile, line);
if(line.find("HIERARCHY") != std::string::npos)
{
hierarchy = true;
}
else if(line.find("OFFSET ") != std::string::npos)
{
hierarchy = false;
line = line.substr(7);
char *endptr;
int h = 0;
offset[h++] = strtod(line.c_str(), &endptr);
for(; h< 3; ++h)
{
double tg = strtod(endptr, &endptr);
offset[h] = tg; // strtod(endptr, &endptr);
}
}
else if(line.find("MOTION") != std::string::npos)
{
hierarchy = false;
}
else if(line.find("Frame Time") != std::string::npos)
{
motion = true;
}
else if(line.find("CONTACT EFFECTORS ") != std::string::npos)
{
motion = false;
contacts = true;
line = line.substr(18);
char *endptr;
nbEffectors = (int)strtod(line.c_str(), &endptr);
}
else if(!line.empty())
{
if(hierarchy)
{
res.push_back(line);
}
else if(motion)
{
Eigen::VectorXd waypoint(res.size());
char *endptr;
unsigned int h = 0;
waypoint[h++] = strtod(line.c_str(), &endptr);
for(; h< res.size(); ++h)
{
waypoint[h] = strtod(endptr, &endptr);
}
for(int i =0; i<3; ++i)
{
waypoint[i] += offset[i];
}
waypoint[2] -= 1;
waypoints.push_back(waypoint);
}
else if(contacts)
{
Eigen::MatrixXd contactPoint(nbEffectors, 8);
char *endptr;
for(int i=0; i< nbEffectors; ++i)
{
int h = 0;
if (i == 0)
contactPoint(i,h++) = strtod(line.c_str(), &endptr);
else
contactPoint(i,h++) = strtod(endptr, &endptr);
for(; h< 8; ++h)
{
contactPoint(i,h) = strtod(endptr, &endptr);
}
for(int k =1; k<4; ++k)
{
contactPoint(i,k) += offset[k];
}
contactPoint(i,3) -= 1;
}
contactPoints.push_back(contactPoint);
}
}
}
myfile.close();
}
else
{
std::cout << "can not find initial path file" << filepath << std::endl;
}
if(!(contactPoints.empty() || waypoints.size() == contactPoints.size()))
{
std::cout << "Error in initial path file: not same number of contacts and frames" << filepath << std::endl;
}
return res;
}