int main ()
{
printf ("Results of cursor_test:\n");
try
{
common::FileSystem::AddSearchPath ("data");
Window window1 (WindowStyle_Overlapped, 400, 300),
window2 (WindowStyle_Overlapped, 400, 300);
window2.SetCursor ("aero_busy.ani");
window1.HideCursor ();
window1.Show ();
window2.Show ();
window1.RegisterEventHandler (WindowEvent_OnClose, &destroy);
window2.RegisterEventHandler (WindowEvent_OnClose, &destroy);
Application::Run ();
return Application::GetExitCode ();
}
catch (std::exception& exception)
{
printf ("exception: %s\n", exception.what ());
}
return 0;
}
int main(int argc, char** argv)
{
ClockModel* model = new ClockModel(0, 0, 0);
ClockApp app(model, argc, argv);
ClockLabelWindow window1(model);
ClockWindow window2(model);
model->setTime(11, 0, 0);
/** 1 second interval. */
app.startTimer(1000);
window1.show();
window1.raise();
window1.activateWindow();
window1.move(300, 400);
window2.show();
window2.raise();
window2.activateWindow();
window2.move(600, 300);
return app.exec();
}
/**
* \brief Radon Transform
*/
cv::Mat radon(const char *filename, int width, int height) {
// read the image
cv::Mat inimg = cv::imread(std::string(filename));
//std::cout << "reading image " << filename << std::endl;
// convert the image to grayscale
cv::Mat ingray(inimg.rows, inimg.cols, CV_64FC1);
cv::cvtColor(inimg, ingray, CV_BGR2GRAY);
// now apply the windowing function (in place)
cv::Point2d center(inimg.cols / 2, inimg.rows / 2);
double r = min(center.x, center.y);
for (int x = 0; x < inimg.cols; x++) {
for (int y = 0; y < inimg.rows; y++) {
double l = hypot(x - center.x, y - center.y) / r;
double f = window2(l);
unsigned char v = ingray.at<unsigned char>(y, x);
v = v * f;
ingray.at<unsigned char>(y, x) = f * v;
}
}
cv::imwrite(std::string("masked.jpg"), ingray);
Grid grid(ingray.cols, ingray.rows);
// create a new image that will take the Radon transform
cv::Mat radon(width, height, CV_64FC1);
// compute the radon transform
double anglestep = M_PI / height;
double srange = 2 * grid.maxS();
double sstep = srange / width;
int y = 0, x = 0;
double norm = hypot(width, height) / 1.5;
for (double angle = 0; y < height; angle += anglestep, y++) {
//std::cout << "y = " << y << std::endl;
x = 0;
for (double s = -srange / 2; x < width; x++, s += sstep) {
GridRay ray(grid, angle, s);
GridIterator i(ray);
double S = 0;
try {
//std::cout << "iterator over ray " << ray << std::endl;
#if 1
for (i = ray.begin(); i != ray.end(); ++i) {
S += i.weight * ingray.at<unsigned char>(i.y, i.x);
}
#endif
} catch (std::exception& x) {
//std::cerr << "exception: " << x.what() << std::endl;
}
unsigned char value = S / norm;
radon.at<double>(y, x) = value;
//std::cout << "[" << x << "," << y << "] = " << S / norm << std::endl;
}
}
// return the result
return radon;
}
int main(int argc,char * argv[])
{
QApplication app(argc,argv);
QDesktopWidget * desktopWidget = QApplication::desktop();
int screenNum = desktopWidget->screenCount();
if (screenNum == 3)
{
QRect rect;
int i;
Dialog window0;
Dialog window1(&window0);
Dialog window2(&window1);
qDebug()<<"window0 "<<window0.winId();
qDebug()<<"window1 "<<window1.winId();
qDebug()<<"window2 "<<window2.winId();
for (i=0;i<screenNum;i++)
{
rect = desktopWidget->screenGeometry(i);
qDebug()<<i<<" "<<rect;
switch(rect.width())
{
case SCREEN0:
{
window0.move(rect.topLeft());
window0.resize(rect.width(),rect.height());
break;
}
case SCREEN1:
{
window1.move(rect.topLeft());
window1.resize(rect.width(),rect.height());
break;
}
case SCREEN2:
{
window2.move(rect.topLeft());
window2.resize(rect.width(),rect.height());
break;
}
}
}
window0.show();
window1.show();
window2.show();
window2.startTimer();
return app.exec();
}
}
void Result() {
extern int level, score, life;
sf::Event event;
sf::Font font;
font.loadFromFile("SourceCodePro.ttf");
sf::Text text2;
text2.setFont(font);
text2.setCharacterSize(20);
text2.setColor(sf::Color::Black);
if (life <= 0) {
sf::RenderWindow window2(sf::VideoMode(300, 80), "Game Over!");
while (window2.isOpen()) {
while (window2.pollEvent(event)) {
if (event.type == sf::Event::Closed)
window2.close();
}
window2.display();
char s2[30];
sprintf(s2, "Your Score: %d\nYour level: Level %d", score, level);
text2.setString(s2);
window2.clear(sf::Color::White);
window2.draw(text2);
}
}
if (score == 7500) {
sf::RenderWindow window3(sf::VideoMode(300, 80), "You Win!");
while (window3.isOpen()) {
while (window3.pollEvent(event)) {
if (event.type == sf::Event::Closed)
window3.close();
}
window3.display();
char s2[30];
sprintf(s2, "Your Score: %d\nYour level: Level %d", score, level);
text2.setString(s2);
window3.clear(sf::Color::White);
window3.draw(text2);
}
}
}
int main ()
{
printf ("Results of scene1_test:\n");
try
{
common::LogFilter log_filter ("render.*", &log_print);
syslib::Window window (syslib::WindowStyle_Overlapped, 512, 512);
syslib::Window window2 (syslib::WindowStyle_Overlapped, 512, 512);
window.RegisterEventHandler (syslib::WindowEvent_OnClose, xtl::bind (&on_window_close));
const char* SERVER_NAME = "MyServer";
Server server (SERVER_NAME, render::scene::server::ServerThreadingModel_SingleThreaded);
Client client (SERVER_NAME);
common::PropertyMap window_properties;
window_properties.SetProperty ("ColorBits", 32);
window_properties.SetProperty ("DepthBits", 24);
server.AttachWindow ("my_window", window, window_properties);
server.AttachWindow ("my_window2", window2, window_properties);
client.LoadResource ("data/render.rfx");
client.LoadResource ("data/u1/texture0000.dds");
client.LoadResource ("data/u1/texture0001.dds");
client.LoadResource ("data/u1/texture0002.dds");
client.LoadResource ("data/u1/EnvGala_020_D.dds");
client.LoadResource ("data/u1.xmtl");
client.LoadResource ("data/u1.xmesh");
client.LoadResource ("data/quad.xmesh");
RenderTarget target = client.CreateRenderTarget ("my_window");
RenderTarget target2 = client.CreateRenderTarget ("my_window2");
scene_graph::Screen screen;
scene_graph::Screen screen2;
target.SetScreen (&screen);
target2.SetScreen (&screen2);
// screen.SetBackgroundState (true);
screen.SetBackgroundColor (math::vec4f (0.0f, 1.0f, 1.0f, 1.0f));
// screen2.SetBackgroundState (true);
screen2.SetBackgroundColor (math::vec4f (1.0f, 0.0f, 1.0f, 1.0f));
scene_graph::OrthoCamera::Pointer camera = scene_graph::OrthoCamera::Create ();
camera->SetLeft (-10.0f);
camera->SetRight (10.0f);
camera->SetBottom (-10.0f);
camera->SetTop (10.0f);
camera->SetZNear (-1000.0f);
camera->SetZFar (1000.0f);
camera->SetPosition (0, 10.0f, 0.0f);
camera->LookTo (math::vec3f (0.0f), scene_graph::NodeOrt_Z, scene_graph::NodeOrt_X, scene_graph::NodeTransformSpace_World);
// camera->LookTo (math::vec3f (0.0f), math::vec3f (0, 1.0f, 0), scene_graph::NodeTransformSpace_World);
scene_graph::Scene scene;
camera->BindToScene (scene);
scene_graph::OrthoCamera::Pointer camera2 = scene_graph::OrthoCamera::Create ();
camera2->SetLeft (-10.0f);
camera2->SetRight (10.0f);
camera2->SetBottom (-10.0f);
camera2->SetTop (10.0f);
camera2->SetZNear (-1000.0f);
camera2->SetZFar (1000.0f);
camera2->SetPosition (0, 10.0f, 0.0f);
camera2->LookTo (math::vec3f (0.0f), scene_graph::NodeOrt_Z, scene_graph::NodeOrt_X, scene_graph::NodeTransformSpace_World);
camera2->BindToScene (scene);
scene_graph::OrthoCamera::Pointer camera3 = scene_graph::OrthoCamera::Create ();
camera3->SetLeft (-10.0f);
camera3->SetRight (10.0f);
camera3->SetBottom (-10.0f);
camera3->SetTop (10.0f);
camera3->SetZNear (-1000.0f);
camera3->SetZFar (1000.0f);
camera3->SetPosition (0, 10.0f, 0.0f);
camera3->LookTo (math::vec3f (0.0f), scene_graph::NodeOrt_Z, scene_graph::NodeOrt_X, scene_graph::NodeTransformSpace_World);
camera3->BindToScene (scene);
scene_graph::OrthoCamera::Pointer camera4 = scene_graph::OrthoCamera::Create ();
camera4->SetLeft (-10.0f);
camera4->SetRight (10.0f);
camera4->SetBottom (-10.0f);
camera4->SetTop (10.0f);
camera4->SetZNear (-1000.0f);
camera4->SetZFar (1000.0f);
camera4->SetPosition (0, 10.0f, 0.0f);
camera4->LookTo (math::vec3f (0.0f), scene_graph::NodeOrt_Z, scene_graph::NodeOrt_X, scene_graph::NodeTransformSpace_World);
camera4->BindToScene (scene);
scene_graph::StaticMesh::Pointer model = scene_graph::StaticMesh::Create ();
model->SetMeshName ("u1.polySurface2.mesh#0");
model->BindToScene (scene);
scene_graph::StaticMesh::Pointer model2 = scene_graph::StaticMesh::Create ();
model2->SetMeshName ("quad");
model2->Rotate (math::degree (-90.f), math::degree (0.f), math::degree (0.f));
model2->SetPosition (0, -8.0f, 0);
model2->Scale (10.0f, 10.0f, 10.0f);
model2->BindToScene (scene);
scene_graph::SpotLight::Pointer light = scene_graph::SpotLight::Create ();
light->SetPosition (0.0f, 10.0f, 0);
light->SetRange (20.0f);
light->LookTo (math::vec3f (0.0f), scene_graph::NodeOrt_Z, scene_graph::NodeOrt_X, scene_graph::NodeTransformSpace_World);
light->SetIntensity (1.0f);
light->SetAngle (math::degree (100.0f));
light->BindToScene (scene);
//пїЅпїЅпїЅпїЅпїЅпїЅпїЅпїЅпїЅ пїЅпїЅпїЅпїЅпїЅпїЅпїЅ пїЅпїЅпїЅпїЅпїЅпїЅ
scene_graph::Viewport vp;
vp.SetArea (0, 0, 100, 100);
vp.SetCamera (camera.get ());
vp.SetTechnique ("default");
vp.SetMinDepth (0);
vp.SetMaxDepth (0.2);
screen.Attach (vp);
scene_graph::Viewport vp2;
vp2.SetArea (0, 0, 100, 100);
vp2.SetCamera (camera2.get ());
vp2.SetTechnique ("default");
vp2.SetMinDepth (0.3);
vp2.SetMaxDepth (0.5);
screen2.Attach (vp2);
window.Show ();
window2.Show ();
window.SetPosition (100, 100);
window2.SetPosition (700, 100);
Test test (target, target2, *model);
syslib::Application::RegisterEventHandler (syslib::ApplicationEvent_OnIdle, xtl::bind (&idle, xtl::ref (test), xtl::ref (*light)));
syslib::Application::Run ();
}
catch (std::exception& e)
{
printf ("%s\n", e.what ());
}
}
int main( ){
USING_NAMESPACE_ACADO
// DEFINE SAMPLE OCP
// -----------------
DifferentialState x, y;
Control u;
DifferentialEquation f;
f << dot(x) == y;
f << dot(y) == u;
OCP ocp( 0.0, 5.0 );
ocp.minimizeLagrangeTerm( x*x + 0.01*u*u );
ocp.subjectTo( f );
ocp.subjectTo( AT_START, x == 1.0 );
ocp.subjectTo( AT_START, y == 0.0 );
ocp.subjectTo( -1.0 <= u <= 1.0 );
// SETUP OPTIMIZATION ALGORITHM AND DEFINE VARIOUS PLOT WINDOWS
// ------------------------------------------------------------
OptimizationAlgorithm algorithm( ocp );
GnuplotWindow window1;
window1.addSubplot( x, "1st state" );
window1.addSubplot( y, "2nd state","x label","y label",PM_POINTS );
window1.addSubplot( u, "control input","x label","y label",PM_LINES,0,5,-2,2 );
window1.addSubplot( 2.0*sin(x)+y*u, "Why not plotting fancy stuff!?" );
// window1.addSubplot( PLOT_KKT_TOLERANCE, "","iteration","KKT tolerance" );
GnuplotWindow window2( PLOT_AT_EACH_ITERATION );
window2.addSubplot( x, "1st state evolving..." );
algorithm << window1;
algorithm << window2;
algorithm.solve( );
// For illustration, an alternative way to plot differential states
// in form of a VariablesGrid
VariablesGrid states;
algorithm.getDifferentialStates( states );
GnuplotWindow window3;
window3.addSubplot( states(0), "1st state obtained as VariablesGrid" );
window3.addSubplot( states(1), "2nd state obtained as VariablesGrid" );
window3.plot();
VariablesGrid data( 1, 0.0,10.0,11 );
data.setZero();
data( 2,0 ) = 1.0;
data( 5,0 ) = -1.0;
data( 8,0 ) = 2.0;
data( 9,0 ) = 2.0;
// data.setType( VT_CONTROL );
GnuplotWindow window4;
window4.addSubplot( data, "Any data can be plotted" );
window4.plot();
return 0;
}
void os3_psr(tom::os3_volume<T>* xcfV,tom::os3_volume<T>* patternV){
tom::Volume<T> psrMask(*xcfV->getVolume());
psrMask.setValues((T)0);
//psrMask.printInfo("psrMask");
std::size_t sx = xcfV->getSizeX();
std::size_t sy = xcfV->getSizeY();
std::size_t sz = xcfV->getSizeZ();
std::size_t px = patternV->getSizeX();
std::size_t py = patternV->getSizeY();
std::size_t pz = patternV->getSizeZ();
std::size_t posX = sx/2-px/2;
std::size_t posY = sy/2-py/2;
std::size_t posZ = sz/2-pz/2;
if(sz <= 1)
posZ = 0;
//create window of same size as the pattern at the center of psrMask
tom::Volume<T> windowVolume(psrMask,&psrMask.get(posX,posY,posZ),
px,py,pz,
psrMask.getStrideX(), psrMask.getStrideY(), psrMask.getStrideZ());
//windowVolume.printInfo("windowVolume");
float outerRadius = patternV->getSizeX()/2;
//create sphere with outer radius size
tom::init_spheremask(windowVolume,outerRadius,(float).0,(float).0);
float innerRadius = patternV->getSizeX() /4;
if(innerRadius <5){
innerRadius = 5;
}
sx = windowVolume.getSizeX()/2;
sy = windowVolume.getSizeY()/2;
sz = windowVolume.getSizeZ()/2;
posX = sx - (std::size_t)innerRadius;
posY = sy - (std::size_t)innerRadius;
posZ = sz - (std::size_t)innerRadius;
if(sz <= 1)
posZ = 0;
//create an volume of same size as the pattern
tom::Volume<T> window2(windowVolume);
window2.setValues((T)0);
//leap into the new volume
tom::Volume<T> smallWindow(window2,&window2.get(posX,posY,posZ),
(std::size_t)innerRadius*2,(std::size_t)innerRadius*2,((sz<=0) ? (1):((std::size_t)innerRadius*2)),
window2.getStrideX(), window2.getStrideY(), window2.getStrideZ());
//create a small sphere there
tom::init_spheremask(smallWindow,innerRadius,(float).0,(float).0);
//create a ring sphere in the big volume (psrMask)
tom::element_wise_sub(windowVolume,window2);
//psrMask.write_to_em("/fs/home/hrabe/develop/images/psr.em",NULL);
//psr mask is finished
//patternV now carries the psrMask
patternV->setVolume(&psrMask);
patternV->replaceCurrentPlan(xcfV);
//std::cout << std::endl << __FILE__ << " " << __LINE__ << std::endl;
tom::os3_volume<T> xcfVCopy(xcfV);
//std::cout << std::endl << __FILE__ << " " << __LINE__ << std::endl;
xcfVCopy.resetStatistics(patternV);
tom::Volume<T>* meanVolume = xcfVCopy.getMeanVolume();
tom::Volume<T>* stdVolume = xcfVCopy.getStdVolume();
tom::Volume<T>* volume = xcfVCopy.getVolume();
tom::element_wise_sub(*volume,*meanVolume);
tom::element_wise_div(*volume,*stdVolume,(T)0.0);
patternV->setVolume(&xcfVCopy);
}
short AssembleOneSV(std::map<std::string,int> &ChrName2Index, ControlState & CurrentState, const Assembly & OneSV, std::ofstream & ASM_Output) {
//std::cout << "AssembleOneSV 1" << std::endl;
//short Max_NT_Size = 30;
bool WhetherFirstBP = true;
std::vector <SPLIT_READ> First, Second;
unsigned SearchCenter;
unsigned SearchRange;
std::cout << "Current SV: " << OneSV.Index << " " << OneSV.Type << " " << OneSV.ChrA << " " << OneSV.PosA << " " << OneSV.CI_A
<< "\t" << OneSV.ChrB << " " << OneSV.PosB << " " << OneSV.CI_B << std::endl;
// get first BP
CurrentState.Reads_SR.clear();
CurrentState.OneEndMappedReads.clear();
//std::cout << "AssembleOneSV 2" << std::endl;
const Chromosome* currentChromosomeFirst = g_genome.getChr(OneSV.ChrA);
const Chromosome* currentChromosomeSecond = g_genome.getChr(OneSV.ChrB);
std::string FirstChrSeq = currentChromosomeFirst->getSeq();
std::string SecondChrSeq = currentChromosomeSecond->getSeq();
if (CurrentState.CurrentChrName != OneSV.ChrA) {
CurrentState.CurrentChrName = OneSV.ChrA;
CurrentState.CurrentChrSeq = FirstChrSeq;
//CurrentState.CurrentChrSeq = AllChromosomes[ChrName2Index.find(OneSV.ChrA)->second].ChrSeq; // change later, copying one chrseq for each SV is expensive.
}
//const unsigned CONS_Chr_Size = currentChromosome->getBiolSize(); // #################
//std::cout << "CONS_Chr_Size " << CONS_Chr_Size << std::endl;
g_maxPos = 0; // #################
unsigned Left, Right;
unsigned int lowerBinBorder = 1;
if (OneSV.PosA > OneSV.CI_A + 1000)
lowerBinBorder = OneSV.PosA - OneSV.CI_A - 1000; //CurrentState.
unsigned int upperBinBorder = OneSV.PosA + OneSV.CI_A + 1000;
SearchWindow window( currentChromosomeFirst, lowerBinBorder, upperBinBorder );
Left = OneSV.PosA + g_SpacerBeforeAfter - OneSV.CI_A;
Right = OneSV.PosA + g_SpacerBeforeAfter + OneSV.CI_A;
std::cout << "\nFirst BP\tChrName " << CurrentState.CurrentChrName << "\tRange " << lowerBinBorder << " " << upperBinBorder << std::endl;
get_SR_Reads(CurrentState, window );
//std::cout << "First size: " << CurrentState.Reads_SR.size() << std::endl;
CombineAndSort(ChrName2Index, CurrentState, OneSV, First, lowerBinBorder, upperBinBorder, WhetherFirstBP);
CleanUpCloseEnd(First, Left, Right); // vector of reads
std::cout << "\nFirst size " << First.size() << std::endl;
SearchRange = OneSV.CI_B + 1000;
SearchCenter = OneSV.PosB + g_SpacerBeforeAfter;
std::vector< SearchWindow > searchCluster;
searchCluster.push_back( SearchWindow( currentChromosomeSecond, SearchCenter-SearchRange, SearchCenter+SearchRange ) );
Left = OneSV.PosB + g_SpacerBeforeAfter - OneSV.CI_B;
Right = OneSV.PosB + g_SpacerBeforeAfter + OneSV.CI_B;
for (unsigned ReadIndex = 0; ReadIndex < First.size(); ReadIndex++) {
First[ReadIndex].FarFragName = OneSV.ChrB;
//SearchFarEndAtPos(AllChromosomes[ChrName2Index.find(OneSV.ChrB)->second].ChrSeq, First[ReadIndex], SearchCenter, SearchRange);
SearchFarEndAtPos(SecondChrSeq, First[ReadIndex], searchCluster);
}
//std::cout << "AssembleOneSV 7" << std::endl;
CleanUpFarEnd(First, Left, Right);
//std::cout << "AssembleOneSV 8" << std::endl;
for (unsigned ReadIndex = 0; ReadIndex < First.size(); ReadIndex++) {
if (First[ReadIndex].UP_Close.size()) {
if (First[ReadIndex].UP_Far.size()) {
OutputCurrentRead(ChrName2Index, CurrentState, OneSV, First[ReadIndex], ASM_Output);
}
}
}
// get second BP
CurrentState.Reads_SR.clear();
WhetherFirstBP = false;
CurrentState.CurrentChrName = OneSV.ChrB;
CurrentState.CurrentChrSeq = SecondChrSeq;
//CONS_Chr_Size = CurrentState.CurrentChrSeq.size() - 2 * g_SpacerBeforeAfter; // #################
g_maxPos = 0; // #################
lowerBinBorder = 1;
if (OneSV.PosB > OneSV.CI_B + 1000)
lowerBinBorder = OneSV.PosB - OneSV.CI_B - 1000;
upperBinBorder = OneSV.PosB + OneSV.CI_B + 1000;
SearchWindow window2( currentChromosomeSecond, lowerBinBorder, upperBinBorder );
Left = OneSV.PosB + g_SpacerBeforeAfter - OneSV.CI_B;
Right = OneSV.PosB + g_SpacerBeforeAfter + OneSV.CI_B;
std::cout << "\nSecond BP\tChrName " << CurrentState.CurrentChrName << "\tRange " << lowerBinBorder << " " << upperBinBorder << std::endl;
get_SR_Reads(CurrentState, window2 );
CombineAndSort(ChrName2Index, CurrentState, OneSV, Second, lowerBinBorder, upperBinBorder, WhetherFirstBP);
CleanUpCloseEnd(Second, Left, Right);
std::cout << "\nSecond size " << Second.size() << std::endl;
SearchRange = OneSV.CI_A + 1000;
SearchCenter = OneSV.PosA + g_SpacerBeforeAfter;
Left = OneSV.PosA + g_SpacerBeforeAfter - OneSV.CI_A;
Right = OneSV.PosA + g_SpacerBeforeAfter + OneSV.CI_A;
searchCluster.clear();
searchCluster.push_back( SearchWindow( currentChromosomeFirst, SearchCenter-SearchRange, SearchCenter+SearchRange ) );
for (unsigned ReadIndex = 0; ReadIndex < Second.size(); ReadIndex++) {
Second[ReadIndex].FarFragName = OneSV.ChrA;
//SearchFarEndAtPos(AllChromosomes[ChrName2Index.find(OneSV.ChrA)->second].ChrSeq, Second[ReadIndex], SearchCenter, SearchRange);
SearchFarEndAtPos(FirstChrSeq, Second[ReadIndex], searchCluster);
}
CleanUpFarEnd(Second, Left, Right);
for (unsigned ReadIndex = 0; ReadIndex < Second.size(); ReadIndex++) {
if (Second[ReadIndex].UP_Close.size()) {
if (Second[ReadIndex].UP_Far.size()) {
OutputCurrentRead(ChrName2Index, CurrentState, OneSV, Second[ReadIndex], ASM_Output);
}
}
}
unsigned SumSize = 0;
for (unsigned ReadIndex = 0; ReadIndex < First.size(); ReadIndex++) {
SumSize += First[ReadIndex].UP_Far.size();
}
for (unsigned ReadIndex = 0; ReadIndex < Second.size(); ReadIndex++) {
SumSize += Second[ReadIndex].UP_Far.size();
}
if (SumSize == 0 && OneSV.ChrA == OneSV.ChrB) {
TryLI(ChrName2Index, CurrentState, OneSV, First, Second, ASM_Output);
}
return 0;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
DifferentialState x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4,arm1x,arm1v,arm2x,arm2v;
Control vu1,vu2,vu3,vu4;
//Parameter T;
DifferentialEquation f(0.0,15);
const double c = 0.00001;
const double Cf = 0.00065;
const double d = 0.250;
const double Jx = 0.018;
const double Jy = 0.018;
const double Jz = 0.026;
const double m = 0.9;
const double g = 9.81;
const double Cx = 0.1;
const double Iarm1 = 0 ;
const double xf = 20.;
const double yf = 0.;
const double zf = 0.;
double coeffU = 0.001;
double coeffX = .01;
// DEFINE A DIFFERENTIAL EQUATION:
// -------------------------------
OCP ocp( 0.0, 15, 60 );
//ocp.minimizeMayerTerm( 1000*coeffX*((x-xf)*(x-xf) + (y-yf)*(y-yf) + (z-zf)*(z-zf)) );
ocp.minimizeLagrangeTerm(coeffX*(sqrt(1+(x-xf)*(x-xf)) + sqrt(1+(y-yf)*(y-yf)) + sqrt(1+(z-zf)*(z-zf))) + coeffU*( sqrt(1+(u1-58.27)*(u1-58.27))+sqrt(1+(u2-58.27)*(u2-58.27))+sqrt(1+(u3-58.27)*(u3-58.27))+sqrt(1+(u4-58.27)*(u4-58.27))));
f << dot(x) == vx;
f << dot(y) == vy;
f << dot(z) == vz;
f << dot(vx) == -Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(theta)/m;
f << dot(vy) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(phi)*cos(theta)/m;
f << dot(vz) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*cos(phi)*cos(theta)/m - g;
f << dot(phi) == p + sin(theta)*r;
f << dot(theta) == cos(phi)*q-sin(phi)*cos(theta)*r;
f << dot(psi) == sin(phi)*q+cos(phi)*cos(theta)*r;
f << dot(p) == (d*Cf*(u1*u1-u2*u2)+(Jy-Jz)*q*r)/Jx;
f << dot(q) == (d*Cf*(u4*u4-u3*u3)+(Jz-Jx)*p*r)/Jy;
f << dot(r) == (c*(u1*u1+u2*u2-u3*u3-u4*u4)+(Jx-Jy)*p*q)/Jz;
f << dot(u1) == vu1;
f << dot(u2) == vu2;
f << dot(u3) == vu3;
f << dot(u4) == vu4;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
//Dynamic
ocp.subjectTo( f );
//Start conditions
ocp.subjectTo( AT_START, x == 0 );
ocp.subjectTo( AT_START, y == 0 );
ocp.subjectTo( AT_START, z == 0 );
ocp.subjectTo( AT_START, vx == 0 );
ocp.subjectTo( AT_START, vy == 0 );
ocp.subjectTo( AT_START, vz == 0 );
ocp.subjectTo( AT_START, phi == 0.0 );
ocp.subjectTo( AT_START, theta == -0. );
ocp.subjectTo( AT_START, psi == -0.);
ocp.subjectTo( AT_START, p == 0.0 );
ocp.subjectTo( AT_START, q == 0.0 );
ocp.subjectTo( AT_START, r == 0.0 );
ocp.subjectTo( AT_START, (d*Cf*(u1*u1-u2*u2)+(Jy-Jz)*q*r)/Jx == 0.0 );
ocp.subjectTo( AT_START, (d*Cf*(u4*u4-u3*u3)+(Jz-Jx)*p*r)/Jy == 0.0 );
ocp.subjectTo( AT_START, (c*(u1*u1+u2*u2-u3*u3-u4*u4)+(Jx-Jy)*p*q)/Jz == 0.0 );
ocp.subjectTo( AT_START, -Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(theta)/m == 0 );
ocp.subjectTo( AT_START, Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(phi)*cos(theta)/m == 0 );
ocp.subjectTo( AT_START, Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*cos(phi)*cos(theta)/m - g == 0 );
//Command constraints
//on each motor
//ocp.subjectTo( (u1*u1+u2*u2-u3*u3-u4*u4) == 0);
ocp.subjectTo( 16 <= u1 <= 95 );
ocp.subjectTo( 16 <= u2 <= 95 );
ocp.subjectTo( 16 <= u3 <= 95 );
ocp.subjectTo( 16 <= u4 <= 95 );
ocp.subjectTo( -100 <= vu1 <= 100 );
ocp.subjectTo( -100 <= vu2 <= 100 );
ocp.subjectTo( -100 <= vu3 <= 100 );
ocp.subjectTo( -100 <= vu4 <= 100 );
//ocp.subjectTo( -15 <= vx <= 15);
//ocp.subjectTo( -0.1 <= z <= 0.1);
//ocp.subjectTo( -0.1 <= y <= 0.1);
//on total power
//ocp.subjectTo( u1*u1+u2*u2+u3*u3+u4*u4 <= 800 );
// DEFINE A PLOT WINDOW:
// ---------------------
GnuplotWindow window2(PLOT_AT_EACH_ITERATION);
window2.addSubplot( z,"DifferentialState z" );
window2.addSubplot( x,"DifferentialState x" );
//window2.addSubplot( y,"DifferentialState y" );
window2.addSubplot( vx,"DifferentialState vx" );
window2.addSubplot( vz,"DifferentialState vz" );
window2.addSubplot( Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*cos(phi)*cos(theta)/m - g,"acc z" );
//window2.addSubplot( vx,"DifferentialState vx" );
window2.addSubplot( -Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(theta)/m,"acc x" );
//window2.addSubplot( vy,"DifferentialState vy" );
//window2.addSubplot( Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(phi)*cos(theta)/m,"acc y" );
window2.addSubplot( u1,"u1" );
window2.addSubplot( u2,"u2" );
window2.addSubplot( u3,"u3" );
window2.addSubplot( u4,"u4" );
//window2.addSubplot( u4+u1+u2+u3,"f tot" );
//window2.addSubplot( vu1,"vu1" );
//window2.addSubplot( vu2,"vu2" );
//window2.addSubplot( vu3,"vu3" );
//window2.addSubplot( vu4,"vu4" );
//window2.addSubplot( phi,"phi" );
//window2.addSubplot( theta,"theta" );
//window2.addSubplot( z,"z" );
//window2.addSubplot( y,"y" );
//window.addSubplot( PLOT_KKT_TOLERANCE,"KKT Tolerance" );
// window.addSubplot( 0.5 * m * v*v,"Kinetic Energy" );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
//algorithm.set(DISCRETIZATION_TYPE, SINGLE_SHOOTING);
//VariableGrid T_init(1
// algorithm.initializeParameters(4.0);
// algorithm.set( INTEGRATOR_TYPE, INT_BDF );
// algorithm.set( INTEGRATOR_TOLERANCE, 1e-6 );
// algorithm.set( KKT_TOLERANCE, 1e-3 );
//algorithm.set( DYNAMIC_SENSITIVITY, FORWARD_SENSITIVITY );
Grid timeGrid(0.0,1.0,41);
VariablesGrid u_init(16, timeGrid);
for (int i = 0 ; i<41 ; i++ ) {
if(i>24 && i<33) {
//srand (time(NULL));
u_init(i,12) = 50;
u_init(i,15) = 35;
u_init(i,14) = 50;
u_init(i,13) = 50;
}/*
if(i>15 && i<25) {u_init(i,12) = 15.0;
u_init(i,15) = 90.0;
u_init(i,14) = 15.0;
u_init(i,13) = 15.0;}
if(i>24 && i<34) {u_init(i,12) = 90.0;
u_init(i,15) = 90.0;
u_init(i,14) = 90.0;
u_init(i,13) = 90.0;}
if(i>34 && i<41) {u_init(i,12) = 90.0;
u_init(i,15) = 15.0;
u_init(i,14) = 15.0;
u_init(i,13) = 15.0;}*/
//u_init(i,8) = i*1.6/40;
}
//u_init.print();
//algorithm.initializeDifferentialStates(u_init);
//algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
algorithm.set( MAX_NUM_ITERATIONS, 2000 );
algorithm.set( KKT_TOLERANCE, 1e-18 );
// algorithm.set( MAX_NUM_INTEGRATOR_STEPS, 4 );
//algorithm << window3;
algorithm << window2;
algorithm.solve();
VariablesGrid states, controls;
algorithm.getDifferentialStates("/tmp/states.txt");
algorithm.getDifferentialStates(states);
algorithm.getControls("/tmp/controls.txt");
algorithm.getControls(controls);
/*VariablesGrid output(5,timeGrid);
for ( int i =0 ; i<41 ; i++ ) {
output(i,0) = states(i,0);
output(i,1) = states(i,3);
if(i>0) output(i,2) = (states(i,3)-states(i-1,3))/0.025;
if(i>1) output(i,3) = (output(i,2)-output(i-1,2));///0.025;
if(i>2) output(i,4) = (output(i,3)-output(i-1,3));///0.025;
}*/
//output.print();
//states.print();
// BlockMatrix sens;
// algorithm.getSensitivitiesX( sens );
// sens.print();
return 0;
}
void BattleLoop(std::vector<Character*> *chrt, int ID){
Character *player = chrt->at(0);
Character *enemy = chrt->at(ID);
//grafica battle
//sf::Texture playerTexture, enemyTexture, battleTexture;
sf::Sprite playerSprite;
sf::Sprite enemySprite;
sf::Sprite battleSprite;
sf::Texture playerTexture;
sf::Texture enemyTexture;
sf::Texture battleTexture;
sf::RectangleShape rectp;
sf::RectangleShape recte;
rectp.setSize(sf::Vector2f(100, 50));
recte.setSize(sf::Vector2f(100, 50));
rectp.setOutlineColor(sf::Color::Red);
recte.setOutlineColor(sf::Color::Red);
rectp.setOutlineThickness(5);
recte.setOutlineThickness(5);
rectp.setPosition(400, 200);
recte.setPosition(20, 10);
sf::Font font;
if (!font.loadFromFile(resourcePath() + "LeagueGothic.otf"))
{
std::cout<< "failed to open font"<<std::endl;
}
sf::Text playerlife;
sf::Text enemylife;
playerlife.setColor(sf::Color::Black);
enemylife.setColor(sf::Color::Black);
playerlife.setFont(font);
enemylife.setFont(font);
playerlife.setCharacterSize(30);
enemylife.setCharacterSize(30);
playerlife.setPosition(402, 200);
enemylife.setPosition(22, 10);
playerlife.setString("hp: " + std::to_string(player->getLife()));
enemylife.setString("hp: " + std::to_string(enemy->getLife()));
std::string playerpng = player->getType() + "battle.png";
if(!playerTexture.loadFromFile(resourcePath() + playerpng))
std::cout<<"Error, could not load "<<playerpng<<std::endl;
playerSprite.setTexture(playerTexture);
playerSprite.setTextureRect(sf::IntRect(0,0,110,128));
playerSprite.setPosition(32,128);
std::string enemypng = std::to_string(enemy->getId()) + ".png";//prendo l'id del mostro perchè la frontalsprite del mostro si chiama ID.png
if(!enemyTexture.loadFromFile(resourcePath() +enemypng))
std::cout<<"Error, could not load "<<enemypng<<std::endl;
enemySprite.setTexture(enemyTexture);
enemySprite.setPosition(32*10,50);
if(!battleTexture.loadFromFile(resourcePath() +"battlebackground.png"))
std::cout<<"Error, could not load battlebackground texture"<<std::endl;
battleSprite.setTexture(battleTexture);
battleSprite.setScale(2.2, 2.4);
bool enemyatk = false;
sf::RenderWindow window2(sf::VideoMode(32*16, 32*8), "Battle!!");
window2.setKeyRepeatEnabled(false);
//window2.setMouseCursorVisible(false);
sf::Event events;
std::cerr<<"Rendering event loop: start"<<std::endl;
window2.clear();
window2.draw(battleSprite);
window2.draw(playerSprite);
window2.draw(rectp);
window2.draw(recte);
window2.draw(enemySprite);
window2.draw(playerlife);
window2.draw(enemylife);
window2.display();
std::cerr<<"Rendering event loop: end"<<std::endl;
while (window2.isOpen()){
window2.clear();
window2.draw(battleSprite);
window2.draw(playerSprite);
window2.draw(rectp);
window2.draw(recte);
window2.draw(enemySprite);
window2.draw(playerlife);
window2.draw(enemylife);
window2.display();
while (window2.pollEvent(events)){
if(events.type == sf::Event::Closed){
window2.close();
}
else if(sf::Keyboard::isKeyPressed(sf::Keyboard::Escape)){
window2.close();
}
if (events.type == sf::Event::KeyPressed) {
if (events.key.code==sf::Keyboard::Space) {
//std::cout << "0 - windowcode: " << window2 << std::endl;
playerlife.setString("hp: " + std::to_string(player->getLife()));
enemylife.setString("hp: " + std::to_string(enemy->getLife()));
int succesnem = 50;//rand()%100;
int succesplay = 50; //rand()%100;
std::cout<<succesnem<<"-----"<<succesplay<<std::endl;
if (succesplay<60) {
enemy->setHp(enemy->getLife()-player->getAttack());
}
enemyatk=true;
if (enemy->getLife()<=0) {
chrt->erase(chrt->begin()+ID);
enemyatk=false;
window2.close();
}
if (enemyatk) {
if (succesnem<60) {
player->setHp(player->getLife()-enemy->getAttack());
}
enemyatk = false;
}
playerlife.setString("hp: " + std::to_string(player->getLife()));
enemylife.setString("hp: " + std::to_string(enemy->getLife()));
if (player->getLife()<=0) {
chrt->erase(chrt->begin());
window2.close();
}
window2.clear();
window2.draw(battleSprite);
window2.draw(playerSprite);
window2.draw(rectp);
window2.draw(recte);
window2.draw(enemySprite);
window2.draw(playerlife);
window2.draw(enemylife);
window2.display();
}
}
/*std::cerr<<"Rendering event loop: start"<<std::endl;
window2->clear();
window2->draw(*battleSprite);
window2->draw(*playerSprite);
window2->draw(*rectp);
window2->draw(*recte);
window2->draw(*enemySprite);
window2->draw(*playerlife);
window2->draw(*enemylife);
window2->display();
*/
}//eventloop
}//gameloop
}
