Allink  v0.1
MatematicaPlane.cpp
1 #include "../include/Matematica.h"
2 
4  P1.Copy(P1e);
5  P2.Copy(P2e);
6  P3.Copy(P3e);
7  Dir21 = P2 - P1;
8  Dir31 = P3 - P1;
9  Dir23 = P2 - P3;
10  for(int d=0;d<3;d++){
11  Dir21[d] = P2[d] - P1[d];
12  Dir31[d] = P3[d] - P1[d];
13  Dir23[d] = P2[d] - P3[d];
14  }
15  Vettore P4e = P1 + Dir21 + Dir31;
16  P4.Copy(&P4e);
17  Norm = Dir21 ^ Dir31;
18  Norm[0] = Dir21.x[1]*Dir31.x[2] - Dir21.x[2]*Dir31.x[1];
19  Norm[1] = Dir21.x[2]*Dir31.x[0] - Dir21.x[0]*Dir31.x[2];
20  Norm[2] = Dir21.x[0]*Dir31.x[1] - Dir21.x[1]*Dir31.x[0];
21  Norm.Normalize();
22  // printf("------\n");
23  // P1.Print();
24  // P2.Print();
25  // Dir21.Print();
26  // Norm.Print();
27  for(int d=0;d<3;d++){
28  if( fabs(Norm[d]) > 0.){
29  InvNorm[d] = 1./Norm[d];
30  IsInf[d] = 0;
31  }
32  else{
33  InvNorm[d] = 0.;
34  IsInf[d] = 1;
35  }
36  }
37  for(int d=0;d<3;d++){
38  Bound[d*2 ] = MIN(P1[d],MIN(P2[d],P3[d]));
39  //Bound[d*2 ] = MIN(P1[d],MIN(P2[d],MIN(P3[d],P4[d])));
40  Bound[d*2+1] = MAX(P1[d],MAX(P2[d],P3[d]));
41  //Bound[d*2+1] = MAX(P1[d],MAX(P2[d],MAX(P3[d],P4[d])));
42  }
43  dPar = - P1[0]*Norm[0] - P1[1]*Norm[1] - P1[2]*Norm[2];
44  mxy[0] = (P1[1] - P2[1])*Inv(P1[0] - P2[0]);
45  qxy[0] = P1[1] - mxy[0]*P1[0];
46  mxz[0] = (P1[2] - P2[2])*Inv(P1[0] - P2[0]);
47  qxz[0] = P1[2] - mxz[0]*P1[0];
48  mxy[1] = (P1[1] - P3[1])*Inv(P1[0] - P3[0]);
49  qxy[1] = P1[1] - mxy[1]*P1[0];
50  mxz[1] = (P1[2] - P3[2])*Inv(P1[0] - P3[0]);
51  qxz[1] = P1[2] - mxz[1]*P1[0];
52  mxy[2] = (P3[1] - P2[1])*Inv(P3[0] - P2[0]);
53  qxy[2] = P1[1] - mxy[2]*P1[0];
54  mxz[2] = (P3[2] - P2[2])*Inv(P3[0] - P2[0]);
55  qxz[2] = P1[2] - mxz[2]*P1[0];
56  Rad = 1.;
57 }
59 //--------------------------------------------------------------
60 double Piano::Distance(Vettore *Pos) {
61  double Dist = fabs(Norm.x[0]*Pos->x[0] + Norm.x[1]*Pos->x[1] + Norm.x[2]*Pos->x[2] + dPar);
62  return Dist;
63 }
65  double Dist = Distance(Pos);
66  Vettore Pos1(Pos->NDim);
67  double Sign = 0.;
68  for(int d=0;d<3;d++){
69  Sign += (P1[d] - Pos->x[d])*Norm[d];
70  }
71  if(Sign > 0) Sign = -1.;
72  else Sign = 1.;
73  for(int d=0;d<3;d++){
74  Pos1[d] = Pos->x[d] - Sign*Dist*Norm.x[d];
75  }
76  return Pos1;
77 }
79  double Len = 0.;
80  for(int d=0;d<v->NDim;d++){
81  Len += v->x[d]*Norm[d];
82  }
83  Vettore Scal(Len*Norm[0],Len*Norm[1],Len*Norm[2]);
84  return Scal;
85 }
87  // Vettore v2(v->NDim);
88  // v2.Copy(v);
89  Vettore n1 = ProjOnNorm(v);
90  // Vettore v1 = 2.*(n1 - v2);
91  Vettore v1(3);
92  for(int d=0;d<3;d++){
93  v1[d] = - v->x[d] + 2.*n1[d];
94  }
95  return v1;
96 }
97 int Piano::Impact(Vettore *Pos,Vettore *Vel) {
98  double Dist = Distance(Pos);
99  if(Dist > Rad) return 0;
100  Vettore PosS = ProjOnSurf(Pos);
101  if(!IsOnSurf(&PosS)) return 0;
102  Vettore Vel1 = Reflect(Vel);
103  Vel->Copy(&Vel1);
104 }
106  if(i == 1) return P1;
107  if(i == 2) return P2;
108  if(i == 3) return P3;
109  if(i == 4) return P4;
110  return P1;
111 }
112 double Piano::Inv(double x){
113  return x != 0. ? 1./x : 100000000000.;
114 }
116  //return IsOnSurf1(P);
117  return IsOnSurf2(P);
118 }
120  if( SameSide(P,&P1,&P2,&P3) && SameSide(P,&P2,&P1,&P3) && SameSide(P,&P3,&P1,&P2)) return 1;
121  return 0;
122 }
124  Vettore P(3); P.Copy(P1);
125  Vettore A(3); A.Copy(A1);
126  Vettore B(3); B.Copy(B1);
127  Vettore C(3); C.Copy(C1);
128  Vettore D1 = (C-B)^(P-B);
129  Vettore D2 = (C-B)^(A-B);
130  double Scal = 0.;
131  for(int d=0;d<3;d++){
132  int NUno = (d+1)%3;
133  int NDue = (d+2)%3;
134  D1[d] = (C[NUno]-B[NUno])*(P[NDue]-B[NDue]) - (C[NDue]-B[NDue])*(P[NUno]-B[NUno]);
135  D2[d] = (C[NUno]-B[NUno])*(A[NDue]-B[NDue]) - (C[NDue]-B[NDue])*(A[NUno]-B[NUno]);
136  Scal += D1[d]*D2[d];
137  }
138  if( Scal >= 0) return 1;
139  return 0;
140  if( D1%D2 >= 0) return 1;
141  return 0;
142 }
143 // TOFIX!!!!
145  Vettore P(3);P.Copy(PA);
146  Vettore DirP1 = P - P1;
147  for(int d=0;d<3;d++){
148  DirP1[d] = P[d] - P1[d];
149  }
150  // double Inv = (Dir21%Dir21)*(Dir31%Dir31) - (Dir21%Dir31)*(Dir31%Dir21);
151  // double v = ( (Dir31%Dir31)*(DirP1%Dir31) - (Dir31%Dir21)*(DirP1%Dir31) )/Inv;
152  // double u = ( (Dir21%Dir21)*(DirP1%Dir21) - (Dir21%Dir31)*(DirP1%Dir21) )/Inv;
153  double sq2 = 0.,sq3 = 0.;
154  double pd32 = 0., pdP3 = 0., pdP2 = 0.;
155  for(int d=0;d<3;d++){
156  sq2 += SQR(Dir21[d]);
157  sq3 += SQR(Dir31[d]);
158  pd32 += Dir21[d]*Dir31[d];
159  pdP3 += Dir31[d]*DirP1[d];
160  pdP2 += Dir21[d]*DirP1[d];
161  }
162  double Inv = 1./(sq2*sq3 - SQR(pd32));
163  double v = (sq3*pdP2 - pd32*pdP3)*Inv;
164  double u = (sq2*pdP3 - pd32*pdP2)*Inv;
165  if(u >= 0. && v >= 0. && (u+v) < 1.) return 1;
166  else return 0;
167 }
Vettore Norm
Normal and inverse to the normal.
void Copy(Vettore *o)
Copy the vector.
Vettore P1
Points defining the plane.
double dPar
d of ax+by+cz+d=0
Vettore GetVertex(int i)
Get vertex.
double Normalize()
Normlizes a Vettore.
Geometrical operations on vectors.
Definition: MatematicaVect.h:9
Vettore ProjOnNorm(Vettore *v)
Project on normal (vector)
int IsOnSurf1(Vettore *P)
If the point is inside the triangle first method.
double * x
Where the data are stored.
int IsInf[3]
If the inverse to the normal is infinite.
Piano(Vettore *P1, Vettore *P2, Vettore *P3)
Allocates.
~Piano()
Frees.
int SameSide(Vettore *P, Vettore *A, Vettore *B, Vettore *C)
Is the orientation of the difference vectors on the same side?
double mxy[3]
Slope.
double Distance(Vettore *P)
Distance.
double Bound[6]
Boundaries.
Vettore Dir21
Direction vectors.
Vettore Reflect(Vettore *V)
Reflect a vector by the normal.
int IsOnSurf2(Vettore *P)
If the point is inside the triangle second method.
double Rad
Radius of the contact.
double Inv(double x)
Calculate the inverse.
int NDim
Dimension allocated.
int Impact(Vettore *P, Vettore *V)
Reflect velocity.
double qxy[3]
Intercept.
int IsOnSurf(Vettore *P)
If the point is inside the triangle.
Vettore ProjOnSurf(Vettore *Pos)
Project on surface (point)