1 #include <extUtil.h>
2
3
normdist(double * p0,double * p1,double * p2,double * p3)4 double normdist( double *p0, double *p1, double *p2, double *p3 )
5 /******************************************************************/
6 /* return distance of p0 to plane p1p2p3 */
7 /******************************************************************/
8 {
9 double v0[3], v1[3], v2[3], v3[3], en[3], fi;
10
11 // normalized normal vector
12 v_result( p1,p2, v1);
13 v_result( p1,p3, v2);
14 v_prod( v1, v2, v3 );
15 v_norm( v3, en );
16
17 // angle between p1p0 and en
18 v_result( p1, p0, v0);
19 fi=v_angle(v0, en );
20
21 /*
22 v_print(v0);
23 v_print(v1);
24 v_print(v2);
25 v_print(v3);
26 v_print(en);
27 printf("fi:%f cos(fi):%f v0:%f\n",fi*180./PI, cos(fi), v_betrag(v0));
28 */
29
30 // normal distance of p0 based on trigonometry G=H*cos(PI-fi)
31 return((v_betrag(v0)*cos(fi)));
32 }
33