1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80
| void anglecommande(DVector &xcom,DVector &xfinal,DVector **alphaprim){
DVector *Pd = new DVector(2);
DVector *Vd = new DVector(2);
(*Pd)[0] = xfinal[0] - xcom[0];
(*Pd)[1] = xfinal[1] - xcom[1];
std::cout<<"Pd dans anglecom\n"<<Pd<<std::endl;
(*Vd)[0] = xcom[2];
(*Vd)[1] = xcom[3];
std::cout<<"Vd dans anglecom\n"<<Vd<<std::endl;
double normPdcube = ( Pd->l2Norm() ) * ( Pd->l2Norm() ) * (Pd->l2Norm() );
std::cout<<"normPd dans anglecom\n"<<Pd<<std::endl;
//(r*tmpM)[0];
DMatrix tmpVd(*Vd);
**alphaprim = ((*Pd)*tmpVd)[0];
std::cout<<"PdxVd dans anglecom\n"<<Vd<<std::endl;
**alphaprim *= ( (*Pd)[0] * (*Vd)[1] - (*Pd)[1] * (*Vd)[0] )*(-5/normPdcube);
//alphaprim = alphaprim *(-5/normPdcube);
std::cout<<"Reste Calcul alphaprim dans anglecom\n"<<Vd<<std::endl;
//std::cout<<"alphaprim\n"<<alphaprim<<std::endl;
DELETE_AND_NULLIFY(Pd);
DELETE_AND_NULLIFY(Vd);
}
//Calculer une commande ça marche aussi,c'est pour en recalculer une autre que ça //craint,j'ai pu le constater en commentant à partir du 2ème calcul dans anglecommande
//Cette fonction marche bien,je l'ai testée indépendamment et elle est nickel,je mets son //prototype quand même vu que je l'utilise
//void Simulrk4(double tinit,double pas,int nbpas,DVector &init,DMatrix &solution);
void simulationaveccommande(double tinit,double pas,int nbpas,int nbcom,DVector &xinit,DVector &xfinal,DMatrix &solution,DVector &commandes){
//DVector minisoltmp(0.,0.,0.,0.);
DVector *xtmp = new DVector(size);
DVector *comtmp = new DVector(1);
//double comtmp;
std::cout<<"xtmp dans simulaveccom\n"<<xtmp<<std::endl;
*xtmp = (0.,0.,0.,0.);
DMatrix *soltmp = new DMatrix(nbpas + 1,size);
std::cout<<"soltmp dans simulaveccom\n"<<soltmp<<std::endl;
//double **comtmp = new double*;
std::cout<<"**comtmp apres declaration\n"<<comtmp<<std::endl;
Simulrk4(tinit,pas,nbpas,xinit,*soltmp);
anglecommande(xinit,xfinal,&comtmp);
for(int i = 0;i <= nbpas;i++){
for(int j = 0;j < size;j++){
(solution.getPtr())[nbpas + i][j] = (soltmp->getPtr())[i][j];
}//for i
}//for j
(commandes.getPtr())[1] = (*comtmp)[0];
*xtmp = solution.extractLin(nbpas + nbpas-1);
DELETE_AND_NULLIFY(comtmp);
DELETE_AND_NULLIFY(xtmp);
DELETE_AND_NULLIFY(soltmp);
} |
Partager