MatematicaPlane.cpp
```00001 #include "../include/Matematica.h"
00002
00003 Piano::Piano(Vettore *P1e,Vettore *P2e,Vettore *P3e){
00004   P1.Copy(P1e);
00005   P2.Copy(P2e);
00006   P3.Copy(P3e);
00007   Dir21 = P2 - P1;
00008   Dir31 = P3 - P1;
00009   Dir23 = P2 - P3;
00010   for(int d=0;d<3;d++){
00011     Dir21[d] = P2[d] - P1[d];
00012     Dir31[d] = P3[d] - P1[d];
00013     Dir23[d] = P2[d] - P3[d];
00014   }
00015   Vettore P4e = P1 + Dir21 + Dir31;
00016   P4.Copy(&P4e);
00017   Norm = Dir21 ^ Dir31;
00018   Norm[0] = Dir21.x[1]*Dir31.x[2] - Dir21.x[2]*Dir31.x[1];
00019   Norm[1] = Dir21.x[2]*Dir31.x[0] - Dir21.x[0]*Dir31.x[2];
00020   Norm[2] = Dir21.x[0]*Dir31.x[1] - Dir21.x[1]*Dir31.x[0];
00021   Norm.Normalize();
00022   // printf("------\n");
00023   // P1.Print();
00024   // P2.Print();
00025   // Dir21.Print();
00026   // Norm.Print();
00027   for(int d=0;d<3;d++){
00028     if( fabs(Norm[d]) > 0.){
00029       InvNorm[d] = 1./Norm[d];
00030       IsInf[d] = 0;
00031     }
00032     else{
00033       InvNorm[d] = 0.;
00034       IsInf[d] = 1;
00035     }
00036   }
00037   for(int d=0;d<3;d++){
00038     Bound[d*2    ] = MIN(P1[d],MIN(P2[d],P3[d]));
00039     //Bound[d*2    ] = MIN(P1[d],MIN(P2[d],MIN(P3[d],P4[d])));
00040     Bound[d*2+1] = MAX(P1[d],MAX(P2[d],P3[d]));
00041     //Bound[d*2+1] = MAX(P1[d],MAX(P2[d],MAX(P3[d],P4[d])));
00042   }
00043   dPar = - P1[0]*Norm[0] - P1[1]*Norm[1] - P1[2]*Norm[2];
00044   mxy[0] = (P1[1] - P2[1])*Inv(P1[0] - P2[0]);
00045   qxy[0] = P1[1] - mxy[0]*P1[0];
00046   mxz[0] = (P1[2] - P2[2])*Inv(P1[0] - P2[0]);
00047   qxz[0] = P1[2] - mxz[0]*P1[0];
00048   mxy[1] = (P1[1] - P3[1])*Inv(P1[0] - P3[0]);
00049   qxy[1] = P1[1] - mxy[1]*P1[0];
00050   mxz[1] = (P1[2] - P3[2])*Inv(P1[0] - P3[0]);
00051   qxz[1] = P1[2] - mxz[1]*P1[0];
00052   mxy[2] = (P3[1] - P2[1])*Inv(P3[0] - P2[0]);
00053   qxy[2] = P1[1] - mxy[2]*P1[0];
00054   mxz[2] = (P3[2] - P2[2])*Inv(P3[0] - P2[0]);
00055   qxz[2] = P1[2] - mxz[2]*P1[0];
00056   Rad = 1.;
00057 }
00058 Piano::~Piano(){}
00059 //--------------------------------------------------------------
00060 double Piano::Distance(Vettore *Pos) {
00061   double Dist = fabs(Norm.x[0]*Pos->x[0] + Norm.x[1]*Pos->x[1] + Norm.x[2]*Pos->x[2] + dPar);
00062   return Dist;
00063 }
00064 Vettore Piano::ProjOnSurf(Vettore *Pos){
00065   double Dist = Distance(Pos);
00066   Vettore Pos1(Pos->NDim);
00067   double Sign = 0.;
00068   for(int d=0;d<3;d++){
00069     Sign += (P1[d] - Pos->x[d])*Norm[d];
00070   }
00071   if(Sign > 0) Sign = -1.;
00072   else Sign = 1.;
00073   for(int d=0;d<3;d++){
00074     Pos1[d] = Pos->x[d] - Sign*Dist*Norm.x[d];
00075   }
00076   return Pos1;
00077 }
00078 Vettore Piano::ProjOnNorm(Vettore *v){
00079   double Len = 0.;
00080   for(int d=0;d<v->NDim;d++){
00081     Len += v->x[d]*Norm[d];
00082   }
00083   Vettore Scal(Len*Norm[0],Len*Norm[1],Len*Norm[2]);
00084   return Scal;
00085 }
00086 Vettore Piano::Reflect(Vettore *v){
00087   // Vettore v2(v->NDim);
00088   // v2.Copy(v);
00089   Vettore n1 = ProjOnNorm(v);
00090   // Vettore v1 = 2.*(n1 - v2);
00091   Vettore v1(3);
00092   for(int d=0;d<3;d++){
00093     v1[d] = - v->x[d] + 2.*n1[d];
00094   }
00095   return v1;
00096 }
00097 int Piano::Impact(Vettore *Pos,Vettore *Vel) {
00098   double Dist = Distance(Pos);
00099   if(Dist > Rad) return 0;
00100   Vettore PosS = ProjOnSurf(Pos);
00101   if(!IsOnSurf(&PosS)) return 0;
00102   Vettore Vel1 = Reflect(Vel);
00103   Vel->Copy(&Vel1);
00104 }
00105 Vettore Piano::GetVertex(int i) {
00106   if(i == 1) return P1;
00107   if(i == 2) return P2;
00108   if(i == 3) return P3;
00109   if(i == 4) return P4;
00110   return P1;
00111 }
00112 double Piano::Inv(double x){
00113   return x != 0. ? 1./x : 100000000000.;
00114 }
00115 int Piano::IsOnSurf(Vettore *P){
00116   //return IsOnSurf1(P);
00117   return IsOnSurf2(P);
00118 }
00119 int Piano::IsOnSurf1(Vettore *P){
00120   if( SameSide(P,&P1,&P2,&P3) && SameSide(P,&P2,&P1,&P3) && SameSide(P,&P3,&P1,&P2)) return 1;
00121   return 0;
00122 }
00123 int Piano::SameSide(Vettore *P1,Vettore *A1,Vettore *B1,Vettore *C1){
00124   Vettore P(3); P.Copy(P1);
00125   Vettore A(3); A.Copy(A1);
00126   Vettore B(3); B.Copy(B1);
00127   Vettore C(3); C.Copy(C1);
00128   Vettore D1 = (C-B)^(P-B);
00129   Vettore D2 = (C-B)^(A-B);
00130   double Scal = 0.;
00131   for(int d=0;d<3;d++){
00132     int NUno = (d+1)%3;
00133     int NDue = (d+2)%3;
00134     D1[d] = (C[NUno]-B[NUno])*(P[NDue]-B[NDue]) - (C[NDue]-B[NDue])*(P[NUno]-B[NUno]);
00135     D2[d] = (C[NUno]-B[NUno])*(A[NDue]-B[NDue]) - (C[NDue]-B[NDue])*(A[NUno]-B[NUno]);
00136     Scal += D1[d]*D2[d];
00137   }
00138   if( Scal >= 0) return 1;
00139   return 0;
00140   if( D1%D2 >= 0) return 1;
00141   return 0;
00142 }
00143 // TOFIX!!!!
00144 int Piano::IsOnSurf2(Vettore *PA){
00145   Vettore P(3);P.Copy(PA);
00146   Vettore DirP1 = P - P1;
00147   for(int d=0;d<3;d++){
00148     DirP1[d] = P[d] - P1[d];
00149   }
00150   // double Inv = (Dir21%Dir21)*(Dir31%Dir31) - (Dir21%Dir31)*(Dir31%Dir21);
00151   // double v = ( (Dir31%Dir31)*(DirP1%Dir31) - (Dir31%Dir21)*(DirP1%Dir31) )/Inv;
00152   // double u = ( (Dir21%Dir21)*(DirP1%Dir21) - (Dir21%Dir31)*(DirP1%Dir21) )/Inv;
00153   double sq2 = 0.,sq3 = 0.;
00154   double pd32 = 0., pdP3 = 0., pdP2 = 0.;
00155   for(int d=0;d<3;d++){
00156     sq2 += SQR(Dir21[d]);
00157     sq3 += SQR(Dir31[d]);
00158     pd32 += Dir21[d]*Dir31[d];
00159     pdP3 += Dir31[d]*DirP1[d];
00160     pdP2 += Dir21[d]*DirP1[d];
00161   }
00162   double Inv = 1./(sq2*sq3 - SQR(pd32));
00163   double v = (sq3*pdP2 - pd32*pdP3)*Inv;
00164   double u = (sq2*pdP3 - pd32*pdP2)*Inv;
00165   if(u >= 0. && v >= 0. && (u+v) < 1.) return 1;
00166   else return 0;
00167 }
```