void FieldSim::drawStepByStep(const Field* pField, const std::string& path) { int lambdasCollected = 0; int score = 0; std::string state = "In progress"; int lambdaCount = pField->getLambdaCount(); int numOfSteps = 0; drawField(pField, 0); if (path.length() > 0) { const Field *oldField = new Field(*pField); for (unsigned int i = 0; i < path.length(); i++) { const Field *newField = calcNextState(oldField, path[i]); delete oldField; oldField = newField; if (!newField->isRobotAlive()) { score = 0; state = "Dead"; break; } else if (*newField->getLift() == *newField->getRobot()) { score += 50 * lambdasCollected - 1; state = "Finished"; drawField(newField, i + 1); break; } else if (lambdaCount > newField->getLambdaCount()) { lambdasCollected++; lambdaCount--; score += 25; } else if (path[i] == 'A') { score += 25 * lambdasCollected; state = "Aborted"; break; } drawField(newField, i + 1); numOfSteps++; score--; } delete oldField; std::cout << "Score: " << score << " NumSteps: " << numOfSteps << " NumLambdas: " << lambdasCollected << " LC: " << pField->getLambdaCount() << " State " << state << "\n"; } }
bool Simulator::oneStep(){ ThreadedObject::oneStep(); if (!currentTime()) gettimeofday(&beginTime, NULL); if (adjustTime){ struct timeval tv; gettimeofday(&tv, NULL); startTimes.push_back(tv); if (startTimes.size() > 1.0/timeStep()){ startTimes.pop_front(); } if (startTimes.size() >= 2){ const struct timeval& first = startTimes.front(); const struct timeval& last = startTimes.back(); int realT = (last.tv_sec - first.tv_sec)*1e6 + (last.tv_usec - first.tv_usec); int simT = timeStep()*(startTimes.size()-1)*1e6; int usec = simT - realT; if (usec > 1000){ usleep(usec); } } } tm_control.begin(); for (unsigned int i=0; i<numBodies(); i++){ BodyRTC *bodyrtc = dynamic_cast<BodyRTC *>(body(i).get()); bodyrtc->writeDataPorts(currentTime()); } for (unsigned int i=0; i<numBodies(); i++){ BodyRTC *bodyrtc = dynamic_cast<BodyRTC *>(body(i).get()); bodyrtc->readDataPorts(); } for (unsigned int i=0; i<numBodies(); i++){ BodyRTC *bodyrtc = dynamic_cast<BodyRTC *>(body(i).get()); bodyrtc->preOneStep(); } for (unsigned int i=0; i<receivers.size(); i++){ receivers[i].tick(timeStep()); } tm_control.end(); #if 1 tm_collision.begin(); checkCollision(collisions); tm_collision.end(); #endif tm_dynamics.begin(); constraintForceSolver.clearExternalForces(); if (m_kinematicsOnly){ for (int i=0; i<numBodies(); i++){ body(i)->calcForwardKinematics(); } currentTime_ += timeStep(); }else{ calcNextState(collisions); } for (unsigned int i=0; i<numBodies(); i++){ BodyRTC *bodyrtc = dynamic_cast<BodyRTC *>(body(i).get()); bodyrtc->postOneStep(); } appendLog(); tm_dynamics.end(); if (m_totalTime && currentTime() > m_totalTime){ struct timeval endTime; gettimeofday(&endTime, NULL); double realT = (endTime.tv_sec - beginTime.tv_sec) + (endTime.tv_usec - beginTime.tv_usec)/1e6; printf("total :%8.3f[s], %8.3f[sim/real]\n", realT, m_totalTime/realT); printf("controller:%8.3f[s], %8.3f[ms/frame]\n", tm_control.totalTime(), tm_control.averageTime()*1000); printf("collision :%8.3f[s], %8.3f[ms/frame]\n", tm_collision.totalTime(), tm_collision.averageTime()*1000); printf("dynamics :%8.3f[s], %8.3f[ms/frame]\n", tm_dynamics.totalTime(), tm_dynamics.averageTime()*1000); for (int i=0; i<numBodies(); i++){ hrp::BodyPtr body = this->body(i); int ntri=0; for (int j=0; j<body->numLinks(); j++){ hrp::Link *l = body->link(j); if (l && l->coldetModel){ ntri += l->coldetModel->getNumTriangles(); } } printf("num of triangles : %s : %d\n", body->name().c_str(), ntri); } fflush(stdout); return false; }else{ return true; } }