//----------------------------------------------------------------------------
// Summary: Constructor for dmEnvironment class loads in all sorts of stuff
// Parameters: cfg_ptr - ifstream containing the necessary parameters to
// initialize this class
// Returns: none
//----------------------------------------------------------------------------
void setEnvironmentParameters(dmEnvironment *env, ifstream &cfg_ptr)
{
char flname[FILENAME_SIZE]; // filename w/ gridded terrain data
CartesianVector gravity;
readConfigParameterLabel(cfg_ptr, "Gravity_Vector");
cfg_ptr >> gravity[0] >> gravity[1] >> gravity[2];
env->setGravity(gravity);
// get terrain model.
readConfigParameterLabel(cfg_ptr, "Terrain_Data_Filename");
readFilename(cfg_ptr, flname);
#ifdef DEBUG
cout << "Terrain data file: " << flname << endl << flush;
#endif
env->loadTerrainData(flname);
// get ground characteristics.
Float constant;
readConfigParameterLabel(cfg_ptr, "Ground_Planar_Spring_Constant");
cfg_ptr >> constant;
env->setGroundPlanarSpringConstant(constant);
readConfigParameterLabel(cfg_ptr, "Ground_Normal_Spring_Constant");
cfg_ptr >> constant;
env->setGroundNormalSpringConstant(constant);
readConfigParameterLabel(cfg_ptr, "Ground_Planar_Damper_Constant");
cfg_ptr >> constant;
env->setGroundPlanarDamperConstant(constant);
readConfigParameterLabel(cfg_ptr, "Ground_Normal_Damper_Constant");
cfg_ptr >> constant;
env->setGroundNormalDamperConstant(constant);
float u_s, u_k;
readConfigParameterLabel(cfg_ptr, "Ground_Static_Friction_Coeff");
cfg_ptr >> u_s;
readConfigParameterLabel(cfg_ptr, "Ground_Kinetic_Friction_Coeff");
cfg_ptr >> u_k;
if (u_k > u_s)
{
cerr << "dmEnvironment warning: u_k > u_s friction coefficient.\n";
}
env->setFrictionCoeffs(u_s, u_k);
#ifdef DM_HYDRODYNAMICS
setEnvironmentParameters((dmEnvironment*)env, cfg_ptr);
Float fluid_density;
readConfigParameterLabel(cfg_ptr, "Fluid_Density");
cfg_ptr >> fluid_density;
env->setFluidDensity(fluid_density);
#endif
}
//----------------------------------------------------------------------------
// Summary:
// Parameters:
// Returns:
//----------------------------------------------------------------------------
int main(int argc, char** argv)
{
int i, j;
glutInit(&argc, argv);
//=========================
char *filename = "simulation.cfg";
if (argc > 1)
{
filename = argv[1];
}
glutInitWindowSize(640,480);
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
glutCreateWindow("DynaMechs Example");
myinit();
mouse = dmGLMouse::dmInitGLMouse();
for (i=0; i<4; i++)
{
for (j=0; j<4; j++)
{
view_mat[i][j] = 0.0;
}
view_mat[i][i] = 1.0;
}
//view_mat[3][2] = -10.0;
camera = new dmGLPolarCamera_zup();
camera->setRadius(8.0);
camera->setCOI(8.0, 10.0, 2.0);
camera->setTranslationScale(0.02f);
// load robot stuff
ifstream cfg_ptr;
cfg_ptr.open(filename);
// Read simulation timing information.
readConfigParameterLabel(cfg_ptr,"Integration_Stepsize");
cfg_ptr >> idt;
if (idt <= 0.0)
{
cerr << "main error: invalid integration stepsize: " << idt << endl;
exit(3);
}
motion_plan_rate = (int) (0.5 + 0.01/idt); // fixed rate of 100Hz
readConfigParameterLabel(cfg_ptr,"Display_Update_Rate");
cfg_ptr >> render_rate;
if (render_rate < 1) render_rate = 1;
// ===========================================================================
// Initialize DynaMechs environment - must occur before any linkage systems
char env_flname[FILENAME_SIZE];
readConfigParameterLabel(cfg_ptr,"Environment_Parameter_File");
readFilename(cfg_ptr, env_flname);
dmEnvironment *environment = dmuLoadFile_env(env_flname);
environment->drawInit();
dmEnvironment::setEnvironment(environment);
// ===========================================================================
// Initialize a DynaMechs linkage system
char robot_flname[FILENAME_SIZE];
readConfigParameterLabel(cfg_ptr,"Robot_Parameter_File");
readFilename(cfg_ptr, robot_flname);
G_robot = dynamic_cast<dmArticulation*>(dmuLoadFile_dm(robot_flname));
G_integrator = new dmIntegRK4();
G_integrator->addSystem(G_robot);
initAquaControl(G_robot);
glutReshapeFunc(myReshape);
glutKeyboardFunc(processKeyboard);
glutSpecialFunc(processSpecialKeys);
glutDisplayFunc(display);
glutIdleFunc(updateSim);
dmGetSysTime(&last_tv);
cout << endl;
cout << " p - toggles dynamic simulation" << endl;
cout << " UP/DOWN ARROW - increases/decreases velocity command" << endl;
cout << "RIGHT/LEFT ARROW - changes heading command" << endl << endl;
glutMainLoop();
return 0; /* ANSI C requires main to return int. */
}
//Считаем энтропию текста, формируем кодовые слова для символом алфавита файла
void FreqOne()
{
UINT8 ch = 0;
UINT8 K = 0;
lengthText = 0;
H = 0;
Lmean = 0;
readFilename();
ArrayInit(ArrAlfa);
while (!feof(fi))
{
fscanf_s(fi, "%c", &ch);
if (feof(fi))
{
break;
}
if (strchr(rusUp, ch))
{
ch = ch - 0xC0 + 0xE0;
ArrAlfa[ch].p += 1.0;
lengthText++;
}
else
if (strchr(rusDown, ch))
{
ArrAlfa[ch].p += 1.0;
lengthText++;
}
else
if (strchr(engUp, ch))
{
ch = ch - 'A' + 'a';
ArrAlfa[ch].p += 1.0;
lengthText++;
}
else
if (strchr(engDown, ch))
{
ArrAlfa[ch].p += 1.0;
lengthText++;
}
else
if (strchr("-.,:!?;", ch))
{
ch = '.';
ArrAlfa[ch].p += 1.0;
lengthText++;
}
else
if (ch == ' ')
{
ArrAlfa[ch].p += 1.0;
lengthText++;
}
else
if (ch == 0xA8 || ch == 0xB8)
{
ch = 0xE5;
ArrAlfa[ch].p += 1.0;
lengthText++;
}
}
fclose(fi);
printf_s("The file was closed\n");
for (int i = 0; i < n_Alfa; i++)
{
if (ArrAlfa[i].p != (float)0)
{
ArrAlfa[i].p = ArrAlfa[i].p / lengthText;
printf_s("P[%c] = %1.4f\n", i, ArrAlfa[i].p);
H = H + ArrAlfa[i].p * log2(ArrAlfa[i].p);
K++;
}
}
Hmax = log2((float)K);
if (Hmax == 0)Hmax = 1;
printf_s("Всего символов в файле: %d\n", lengthText);
printf("Всего символов в алфавите: %d\n", K);
printf("Энтропия файла: %3.3f\n", (-1)*H);
printf("Максимальная Энтропия файла: %3.3f\n", Hmax);
printf("Избыточность текста составила: %3.3f\n", (float)1 + H / Hmax);
Gilbert_Mur();
system("pause");
for (int i = 0; i < n_Alfa; i++)
{
if (ArrAlfa[i].p != (float)0)
{
printf_s("P[%c] = %1.4f kod = ", i, ArrAlfa[i].p);
for (int j = 0; j<ArrAlfa[i].length; j++) printf("%c", ArrAlfa[i].code[j]);
printf_s("\n");
}
}
printf("Средняя длина кодового слова: %3.3f\n", Lmean);
printf("Избыточность кодирования составила: %3.3f\n", Lmean - (-1.0)*H);
system("pause");
}
void WriteCodeFile()
{
int numclosed;
errno_t err;
UINT8 ch = 0;
readFilename();
err = fopen_s(&fcode, "code.txt", "wb");
if (err == 0)
{
printf_s("The file CODE.txt was opened\n");
}
else
{
printf_s("The file CODE.txt was not opened\n");
numclosed = _fcloseall();
}
while (!feof(fi))
{
fscanf_s(fi, "%c", &ch);
if (feof(fi))
{
break;
}
if (strchr(rusUp, ch))
{
ch = ch - 0xC0 + 0xE0;
fwrite(ArrAlfa[ch].code, ArrAlfa[ch].length, 1, fcode);
}
else
if (strchr(rusDown, ch))
{
fwrite(ArrAlfa[ch].code, ArrAlfa[ch].length, 1, fcode);
}
else
if (strchr(engUp, ch))
{
ch = ch - 'A' + 'a';
fwrite(ArrAlfa[ch].code, ArrAlfa[ch].length, 1, fcode);
}
else
if (strchr(engDown, ch))
{
fwrite(ArrAlfa[ch].code, ArrAlfa[ch].length, 1, fcode);
}
else
if (strchr("-.,:!?;", ch))
{
ch = '.';
fwrite(ArrAlfa[ch].code, ArrAlfa[ch].length, 1, fcode);
}
else
if (ch == ' ')
{
fwrite(ArrAlfa[ch].code, ArrAlfa[ch].length, 1, fcode);
}
else
if (ch == 0xA8 || ch == 0xB8)
{
ch = 0xE5;
fwrite(ArrAlfa[ch].code, ArrAlfa[ch].length, 1, fcode);
}
}
numclosed = _fcloseall();
system("pause");
}
int main() {
char line[BUFSIZ];
int row[BUFSIZ];
int opt = 0;
FILE* file= NULL;
//char* fn = NULL;
int ncol= 0;
int tmp = 0;
graph* g = NULL;
int* ap = NULL; // pointer within g->adjmtx
router* r = NULL;
int fromto[2] = { 0, 0 };
for (;;) {
printf("\n%s\n", menu);
switch (readNat(&opt)) {
case 0:
freegraph(&g);
goto END;
case 1:
printf("%s\n", prompt1);
//fn = readFilename();
if ((file = fopen(readFilename(), "r")) == NULL) {
printf("File not found.\n");
break;
}
// file is opened
while (getaline(line, BUFSIZ, file)) {//printf("while ...\n");
tmp = 0;
if ((tmp = linetoints(line, row)) > 0) {
if (ncol == 0) {// 1st row
ncol = tmp;
if (g != NULL)
freegraph(&g); // g == NULL
g = makegraph(ncol);
ap = g->adjmtx + intcpy(g->adjmtx, row, ncol);
} else if (ncol == tmp) {
ap += intcpy(ap, row, ncol);
} else {
printf("File content invalid: different number of elements on different rows.\n");
break;
}
}
}
ncol = 0;
fclose(file);
//if (isvalidgraph(g)) {// graph is created
printf("Original routing table is as follows:\n");
printgraph(g);
/*} else {
freegraph(&g);
printf("File content invalid: elements on upper-left to bottom-right diagonal are not all-zero, ");
printf("or, the upper-right half of the matrix and the bottom-left half are not symmetric.\n");
}*/
break;
case 2:
if (g == NULL) {
printf("There is no data currently.\nPlease use option 1 to read a routing table data file first.\n");
break;
}
printf("%s\n", prompt2);
if (readNat(&tmp) < 0 || tmp > g->size) {
printf("Not a valid router id.\n");
break;
}
r = makerouter(tmp, g->size);
buildtable(r, g);
printf("The routing table for router %d is:\n", r->id);
printrouter(r);
freerouter(r);
break;
case 3:
if (g == NULL) {
printf("There is no data currently.\nPlease use option 1 to read a routing table data file first.\n");
break;
}
printf("%s\n", prompt3);
if (read2Nats(fromto) < 0 || fromto[0] > g->size || fromto[1] > g->size) {
printf("Invalid input. Please follow the format:\n\tvalid_src_router_id valid_dest_router_id.\n");
break;
}
// fromto[2] is valid
r = makerouter(fromto[0], g->size);
dijkstra(g, r->id, r->varray);
printpath(r, fromto[1]);
freerouter(r);
break;
default:
printf("%s\n", prompt0);
break;
}
}
END:
return 0;
}
bool MyApp::OnInit()
{
wxCmdLineParser parser(argc,argv);
parser.AddOption(wxT("c"), wxT("config-file"));
parser.Parse();
simThread = new SimulationThread();
simThread->Create();
simThread->SetPriority(100);
// Populate GUI
{
//cout << "Frame" << endl;
frame = new MainFrame(wxT("Huboplus Control Test 1"), wxPoint(50,50), wxSize(600,400));
}
frame->Show();
wxClientDC(frame->glPane);
frame->glPane->SetCurrent();
// Load configurations
{
// load robot stuff
const char *filename;
wxString configFileName;
if(parser.Found(wxT("c"), &configFileName))
{
//filename = configFileName.mb_str(); //not working on Linux
char cstring[256];
strncpy(cstring, (const char*)configFileName.mb_str(wxConvUTF8), 255);
filename = (const char *) cstring;
}
else
{
cout<< "Error: cfg file not supplied !\n\n";
exit(1);
}
cout<<filename<<endl;
ifstream cfg_ptr;
cfg_ptr.open(filename);
Float tmp;
// Read simulation timing information.
readConfigParameterLabel(cfg_ptr,"Integration_Stepsize");
cfg_ptr >> tmp;
simThread->idt = tmp;
if (simThread->idt <= 0.0)
{
cerr << "main error: invalid integration stepsize: " << simThread->idt << endl;
exit(3);
}
readConfigParameterLabel(cfg_ptr,"Control_Stepsize");
cfg_ptr >> tmp;
simThread->cdt = tmp;
simThread->last_control_time = -2*simThread->cdt;
if (simThread->cdt <= 0.0)
{
cerr << "main error: invalid control stepsize: " << simThread->cdt << endl;
exit(3);
}
readConfigParameterLabel(cfg_ptr,"Display_Update_Rate");
cfg_ptr >> frame->glPane->render_rate;
if (frame->glPane->render_rate < 1) frame->glPane->render_rate = 1;
// ------------------------------------------------------------------
// Initialize DynaMechs environment - must occur before any linkage systems
char env_flname[FILENAME_SIZE];
readConfigParameterLabel(cfg_ptr,"Environment_Parameter_File");
readFilename(cfg_ptr, env_flname);
dmEnvironment *environment = dmuLoadFile_env(env_flname);
dmEnvironment::setEnvironment(environment);
// ------------------------------------------------------------------
// Initialize a DynaMechs linkage system
char robot_flname[FILENAME_SIZE];
readConfigParameterLabel(cfg_ptr,"Robot_Parameter_File");
readFilename(cfg_ptr, robot_flname);
cout<<"Robot parameter file: [\033[1;34m"<< robot_flname<< "\033[0m]"<<endl;
G_robot = dynamic_cast<dmArticulation*>(dmuLoadFile_dm(robot_flname));
cout << "Robot created" << endl;
huboCtrl = (HuboController *) new HuboBalanceController(G_robot);
cout << "Robot controller created" << endl;
// set status bar
wxString s;
s.Printf(wxT("idt = %1.1e s"), simThread->idt);
frame->SetStatusText(s,1);
s.Printf(_T("cdt = %f s" ), simThread->cdt);
frame->SetStatusText(s,2);
s.Printf(_T("refresh every %.2f ms"), 1000/(frame->glPane->render_rate) );
frame->SetStatusText(s,3);
// ------------------------
// Read in data directory
char data_dir[FILENAME_SIZE];
readConfigParameterLabel(cfg_ptr, "Data_Save_Directory");
readFilename(cfg_ptr, data_dir);
// std::string(data_dir)
// ...
cout<<"Add robot to integrator "<<endl;
simThread->G_integrator->addSystem(G_robot);
}
// -----------------------
{
cout<<"Initilize scene..."<<endl;
frame->glPane->camera->setRadius(8.0);
frame->glPane->camera->setCOI(3.0, 3.0, 0.0);
frame->glPane->camera->setTranslationScale(0.02f);
frame->glPane->glInit();
dmEnvironment::getEnvironment()->drawInit();
frame->glPane->model_loaded = true;
}
cout << "Restart Timer" << endl;
frame->glPane->restartTimer();
cout << "Start Sim Thread" << endl;
simThread->Run();
return true;
}
bool MyApp::OnInit()
{
wxCmdLineParser parser(argc,argv);
parser.AddOption(wxT("c"), wxT("Config File"));
parser.Parse();
simThread = new SimulationThread();
simThread->Create();
simThread->SetPriority(100);
// Populate GUI
{
//cout << "Frame" << endl;
frame = new MainFrame(wxT("Hello GL World"), wxPoint(50,50), wxSize(600,400));
}
frame->Show();
wxClientDC(frame->glPane);
frame->glPane->SetCurrent();
// Load DM File
{
// load robot stuff
const char *filename;
wxString configFileName;
if(parser.Found(wxT("c"), &configFileName))
{
filename = "humanoid.cfg";//configFileName.mb_str();
}
else {
filename = (char *) "config.cfg";
}
cout<<filename<<endl;
ifstream cfg_ptr;
cfg_ptr.open(filename);
Float tmp;
// Read simulation timing information.
readConfigParameterLabel(cfg_ptr,"Integration_Stepsize");
cfg_ptr >> tmp;
simThread->idt = tmp;
if (simThread->idt <= 0.0)
{
cerr << "main error: invalid integration stepsize: " << simThread->idt << endl;
exit(3);
}
readConfigParameterLabel(cfg_ptr,"Control_Stepsize");
cfg_ptr >> tmp;
simThread->cdt = tmp;
simThread->last_control_time = -2*simThread->cdt;
if (simThread->cdt <= 0.0)
{
cerr << "main error: invalid control stepsize: " << simThread->cdt << endl;
exit(3);
}
readConfigParameterLabel(cfg_ptr,"Display_Update_Rate");
cfg_ptr >> frame->glPane->render_rate;
if (frame->glPane->render_rate < 1) frame->glPane->render_rate = 1;
// ------------------------------------------------------------------
// Initialize DynaMechs environment - must occur before any linkage systems
char env_flname[FILENAME_SIZE];
readConfigParameterLabel(cfg_ptr,"Environment_Parameter_File");
readFilename(cfg_ptr, env_flname);
dmEnvironment *environment = dmuLoadFile_env(env_flname);
dmEnvironment::setEnvironment(environment);
// ------------------------------------------------------------------
// Initialize a DynaMechs linkage system
char robot_flname[FILENAME_SIZE];
readConfigParameterLabel(cfg_ptr,"Robot_Parameter_File");
readFilename(cfg_ptr, robot_flname);
G_robot = dynamic_cast<dmArticulation*>(dmuLoadFile_dm(robot_flname));
humanoid = (HumanoidDataLogger *) new BalanceDemoStateMachine(G_robot);
// --------
// Read in data directory
char data_dir[FILENAME_SIZE];
readConfigParameterLabel(cfg_ptr, "Data_Save_Directory");
readFilename(cfg_ptr, data_dir);
humanoid->dataSaveDirectory = std::string(data_dir);
simThread->G_integrator->addSystem(G_robot);
grfInfo.localContacts = 0;
}
// -- Project specific --
//initControl();
// -----------------------
{
cout<<"initilize scene..."<<endl;
frame->glPane->camera->setRadius(8.0);
frame->glPane->camera->setCOI(3.0, 3.0, 0.0);
frame->glPane->camera->setTranslationScale(0.02f);
frame->glPane->glInit();
dmEnvironment::getEnvironment()->drawInit();
frame->glPane->model_loaded = true;
}
frame->glPane->restartTimer();
simThread->Run();
return true;
}
