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