00001
00002 #include <iostream>
00003
00004 #include "pi2d14/pi2d14_molecule_cube.h"
00005
00006 #include "Atom.h"
00007 #include "ConfigReader.h"
00008 #include "Math.h"
00009
00010
00011 ALGOATOMD::MoleculeCube::MoleculeCube(int buildParam, int typeDep)
00012 {
00013 size = 1 + buildParam;
00014
00015
00016
00017 atoms = new API::Atom[size*size*size];
00018 for(int x=0;x<size;x++)
00019 {
00020 for(int y=0;y<size;y++)
00021 {
00022 for(int z=0;z<size;z++)
00023 {
00024 int num = x + y*size + z*size*size;
00025 atoms[num].util->setCartesianPosition(new API::CartesianPosition( atomSize+2*x*atomSize, atomSize+2*y*atomSize, atomSize+2*z*atomSize) );
00026 }
00027 }
00028 }
00029 linkAllLegs();
00030
00031 m1=false;
00032 m2=false;
00033 m3=false;
00034 indexInit=-1;
00035 index1=-1;
00036 index2=-1;
00037 index3=-1;
00038
00039 typeDeplacement = typeDep;
00040
00041 switch(typeDeplacement)
00042 {
00043 case 1:
00044 initDep1();
00045 break;
00046 default:
00047
00048 break;
00049 }
00050 }
00051
00052 ALGOATOMD::MoleculeCube::~MoleculeCube()
00053 {
00054 delete [] atoms;
00055 }
00056
00057 void ALGOATOMD::MoleculeCube::linkAllLegs()
00058 {
00059
00060 API::Atom *atom1;
00061 API::Atom *atom2;
00062
00063 for(int a1=0;a1<size*size*size;a1++)
00064 {
00065 for(int a2=0;a2<size*size*size;a2++)
00066 {
00067
00068 if(a1!=a2)
00069 {
00070 atom1 = &atoms[a1];
00071 atom2 = &atoms[a2];
00072 for(int p1=0;p1<6;p1++)
00073 {
00074 if(!atom1->getLeg(p1)->isLinked())
00075 {
00076 for(int p2=0;p2<6;p2++)
00077 {
00078
00079
00080 if( !atom2->getLeg(p2)->isLinked() )
00081 {
00082 if( atom2->getLeg(p2)->util->isConnectable(atom1->getLeg(p1)) )
00083 {
00084 atom2->getLeg(p2)->setLink(atom1->getLeg(p1));
00085 }
00086 }
00087 }
00088 }
00089 }
00090 }
00091 }
00092 }
00093 }
00094
00095 void ALGOATOMD::MoleculeCube::processMovement()
00096 {
00097 switch(typeDeplacement)
00098 {
00099 case 1:
00100 algoDep1();
00101 break;
00102 default:
00103
00104 break;
00105 }
00106 }
00107
00108 void ALGOATOMD::MoleculeCube::renderMoleculeInfo()
00109 {
00110 }
00111
00112 void ALGOATOMD::MoleculeCube::initDep1()
00113 {
00114 }
00115
00116 void ALGOATOMD::MoleculeCube::algoDep1()
00117 {
00118 if(!m1 && !m2&& !m3)
00119 {
00120
00121 if(atoms[0].getMolecule()->isFinished(indexInit))
00122 {
00123 printf("11\n");
00124
00125 index1 = atoms[0].getMolecule()->getNextSyncIndex();
00126
00127
00128
00129 for(int x=1;x<size;x+=2)
00130 {
00131
00132 for(int y=1;y<size;y+=2)
00133 {
00134 int num = x + y*size;
00135 atoms[num].getLeg(2)->getServoMotor(0)->setTargetPosition(1,REPLACE,0,index1);
00136 }
00137 }
00138
00139
00140 for(int x=0;x<size;x+=2)
00141 {
00142
00143 for(int y=0;y<size;y+=2)
00144 {
00145 int num = x + y*size;
00146 atoms[num].getLeg(2)->getServoMotor(0)->setTargetPosition(1,REPLACE,0,index1);
00147 }
00148 }
00149
00150
00151 m1=true;
00152 m2=false;
00153 m3=false;
00154
00155
00156 atoms[0].getMolecule()->releaseSyncIndex(indexInit);
00157 }
00158 }
00159
00160 if(m1 && !m2 && !m3)
00161 {
00162 if(atoms[0].getMolecule()->isFinished(index1))
00163 {
00164 printf("22\n");
00165 index2 = atoms[0].getMolecule()->getNextSyncIndex();
00166
00167
00168
00169 for(int x=1;x<size;x+=2)
00170 {
00171
00172 for(int y=0;y<size;y+=2)
00173 {
00174 int num = x + y*size;
00175 atoms[num].getLeg(2)->getServoMotor(0)->setTargetPosition(249,REPLACE,0,index2);
00176 }
00177 }
00178
00179
00180 for(int x=0;x<size;x+=2)
00181 {
00182
00183 for(int y=1;y<size;y+=2)
00184 {
00185 int num = x + y*size;
00186 atoms[num].getLeg(2)->getServoMotor(0)->setTargetPosition(249,REPLACE,0,index2);
00187 }
00188 }
00189
00190
00191 m1=false;
00192 m2=true;
00193 m3=false;
00194
00195 atoms[0].getMolecule()->releaseSyncIndex(index1);
00196 }
00197 }
00198
00199 if(!m1 && m2 && !m3)
00200 {
00201 if(atoms[0].getMolecule()->isFinished(index2))
00202 {
00203 printf("33\n");
00204 index3 = atoms[0].getMolecule()->getNextSyncIndex();
00205
00206
00207
00208 for(int x=1;x<size;x+=2)
00209 {
00210
00211 for(int y=1;y<size;y+=2)
00212 {
00213 int num = x + y*size;
00214 atoms[num].getLeg(2)->getServoMotor(0)->setToHalfPosition(REPLACE,0,index3);
00215 }
00216 }
00217
00218
00219 for(int x=0;x<size;x+=2)
00220 {
00221
00222 for(int y=0;y<size;y+=2)
00223 {
00224 int num = x + y*size;
00225 atoms[num].getLeg(2)->getServoMotor(0)->setToHalfPosition(REPLACE,0,index3);
00226 }
00227 }
00228
00229
00230 m1=false;
00231 m2=false;
00232 m3=true;
00233
00234 atoms[0].getMolecule()->releaseSyncIndex(index2);
00235 }
00236 }
00237
00238 if(!m1 && !m2 && m3)
00239 {
00240 if(atoms[0].getMolecule()->isFinished(index3))
00241 {
00242 printf("ii\n");
00243 indexInit = atoms[0].getMolecule()->getNextSyncIndex();
00244
00245
00246
00247 for(int x=1;x<size;x+=2)
00248 {
00249
00250 for(int y=0;y<size;y+=2)
00251 {
00252 int num = x + y*size;
00253 atoms[num].getLeg(2)->getServoMotor(0)->setToHalfPosition(REPLACE,0,index2);
00254 }
00255 }
00256
00257
00258 for(int x=0;x<size;x+=2)
00259 {
00260
00261 for(int y=1;y<size;y+=2)
00262 {
00263 int num = x + y*size;
00264 atoms[num].getLeg(2)->getServoMotor(0)->setToHalfPosition(REPLACE,0,index2);
00265 }
00266 }
00267
00268 m1=false;
00269 m2=false;
00270 m3=false;
00271
00272 atoms[0].getMolecule()->releaseSyncIndex(index3);
00273 }
00274 }
00275 }
00276
00277
00278 bool ALGOATOMD::MoleculeCube::needMoreMouvement()
00279 {
00280 bool ret = true;
00281 API::CartesianPosition destination = *this->getDestination();
00282
00283
00284 double x1 = atoms[0].util->getCartesianPosition().getX();
00285 double y1 = atoms[0].util->getCartesianPosition().getY();
00286 double x2 = atoms[size*size-1].util->getCartesianPosition().getX();
00287 double y2 = atoms[size*size-1].util->getCartesianPosition().getY();
00288 xg = x1 + (x2-x1)/2;
00289 yg = y1 + (y2-y1)/2;
00290
00291 double demiCote = size*atomSize;
00292
00293 if( (xg+demiCote)>destination.getX() && (xg-demiCote)<destination.getX() && (yg-demiCote)<destination.getY() && (yg+demiCote)>destination.getY() )
00294 {
00295 ret = false;
00296 }
00297
00298 return ret;
00299 }
00300
00301