//Check for passes through the left and right wall
vector<stick> make_ghost_lr( const vector<stick> &m, double LStick, int NumberMikado)
{
std::vector<stick> GhostLR;
for (int i=0;i<NumberMikado;i++){
if(m[i].th!=pi/2 || m[i].th!=3*pi/2){ // Check here for the left wall
stick ghostRowLR;
Matrix2d A; //The matrix to solve with
Vector2d b,b2; // A*st=b
Vector2d st,st2;
A<<-cos(m[i].th),0,-sin(m[i].th),1;
b<<m[i].x,m[i].y;
b2<<m[i].x-1,m[i].y;
st=A.lu().solve(b);
st2=A.lu().solve(b2);
if((st(0)>0 && st(0)<LStick) && (st(1)>0&&st(1)<1)){ //If mikado passes throug wall make ghost mikado
ghostRowLR=m[i];
ghostRowLR.x=ghostRowLR.x+1;
ghostRowLR.wlr=ghostRowLR.wlr-1;
GhostLR.push_back(ghostRowLR);
}
if((st2(0)>0&&st2(0)<LStick)&&(st2(1)>0&&st2(1)<1)){ //Now do the same for the upper wall
ghostRowLR=m[i];
ghostRowLR.x=ghostRowLR.x-1;
ghostRowLR.wlr=ghostRowLR.wlr+1;
GhostLR.push_back(ghostRowLR);
}
}
}
return GhostLR;
}
//Check for passes through the down and up wall;
vector<stick> make_ghost_ud(const vector<stick> &m, double LStick, int NumberMikado){
vector<stick> GhostUD;
for(int i=0;i<NumberMikado;i++){
if(m[i].th!=0||m[i].th!=pi){
stick ghostRowUD;
Matrix2d A;
Vector2d b,b2;
Vector2d st, st2;
A<<-cos(m[i].th),1,-sin(m[i].th),0;
b<<m[i].x,m[i].y;
b2<<m[i].x,m[i].y-1;
st=A.lu().solve(b);
st2=A.lu().solve(b2);
if((st(0)>0&&st(0)<LStick)&&(st(1)>0&&st(1)<1)){
ghostRowUD=m[i];
ghostRowUD.y=ghostRowUD.y+1;
ghostRowUD.wud=ghostRowUD.wud-1;
GhostUD.push_back(ghostRowUD);
}
if((st2(0)>0&&st2(0)<LStick)&&(st2(1)>0&&st2(1)<1)){
ghostRowUD=m[i];
ghostRowUD.y=ghostRowUD.y-1;
ghostRowUD.wud=ghostRowUD.wud+1;
GhostUD.push_back(ghostRowUD);
}
}
}
return GhostUD;
}
//Checks whether the mikado's are connected
void make_connections(vector<connected> &Connection,
const vector<stick> &m,
double LStick,
const vector<spring> &background,
const VectorXd &XYb)
{
Matrix2d A; //The matrix to solve with
Vector2d b,st; // A*st=b
int nr=0;
connected xtrarow, xtrarow2;
//Loop over all sticks to find coordinates
for(int i=0;i<m.size()-1;i++){
for(int j=i+1;j<m.size();j++){
if(m[i].th!=m[j].th || m[i].nr!=m[j].nr){
A<<-cos(m[i].th),cos(m[j].th),-sin(m[i].th),sin(m[j].th);
b<<m[i].x-m[j].x,m[i].y-m[j].y;
st=A.lu().solve(b);
if ((st(0)>0.0 && st(0)<LStick)&&(st(1)>0.0 && st(1)<LStick)){
xtrarow.first=m[i].nr; //[stick i stick j sij sji]
xtrarow.second=m[j].nr;
xtrarow.s1=st(0);
xtrarow.s2=st(1);
xtrarow.nrCon=nr;
xtrarow.recur=0;
xtrarow.type=0;
xtrarow.backgroundspring[0]=-1;
xtrarow.backgroundspring[1]=-1;
xtrarow2.first=m[j].nr; //[stick j sticki sji sij]
xtrarow2.second=m[i].nr;
xtrarow2.s1=st(1);
xtrarow2.s2=st(0);
xtrarow2.nrCon=nr;
xtrarow2.recur=0;
xtrarow2.type=0;
xtrarow2.backgroundspring[0]=-1;
xtrarow2.backgroundspring[1]=-1;
Connection.push_back(xtrarow);
Connection.push_back(xtrarow2);
nr++;
}
}
}
}
//Now loop over the background springs
int one;
int two;
int number=XYb.size()/2;
double xsb,xse,ysb,yse,xmik,ymik;
double thspr, thmik;
double lenspring; //the coordinates + parameters of the spring
double s,t;
if(background.size()>0){//check wheather this block needs to be executed
for(int i=0; i<m.size();i++){
xmik=m[i].x;
ymik=m[i].y;
thmik=m[i].th;
for(int j=0;j<background.size();j++){
//calculate the intersections similar to the previous
one=background[j].one;
two=background[j].two;
xsb=XYb(one);
ysb=XYb(one+number);
xse=XYb(two)+background[j].wlr;
yse=XYb(two+number)+background[j].wud;
thspr=atan2((yse-ysb),(xse-xsb));
lenspring=sqrt(pow((yse-ysb),2)+pow((xse-xsb),2));
if(fabs(thspr-thmik)>1e-10 && fabs(fabs(thspr-thmik)-pi)>1e-10){
A<<cos(thmik),-cos(thspr),sin(thmik),-sin(thspr);
b<<xsb-xmik,ysb-ymik;
st=A.lu().solve(b);
s=st(0);
t=st(1);
if(s>0.0 && t>0.0 && s<LStick && t<lenspring){
//then we found a node!
xtrarow.first=m[i].nr;
xtrarow.second=-1; //the signature of a collision w spring
xtrarow.s1=s;
xtrarow.s2=t;
xtrarow.nrCon=nr;
xtrarow.type=1;
xtrarow.backgroundspring[0]=one;
xtrarow.backgroundspring[1]=two;
xtrarow.recur=0;
Connection.push_back(xtrarow);
nr++;
}
}
}
}
}
}