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
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
|
vector_t initVector (float x, float y, float z) {
vector_t vec={x, y, z};
return vec;
}
vector_t normalizeVector(vector_t v) {
float r=sqrtf(v.c[0]*v.c[0]+v.c[1]*v.c[1]+v.c[2]*v.c[2]);
v.c[0]/=r;
v.c[1]/=r;
v.c[2]/=r;
return v;
};
plane_t initPlane (vector_t normal, vector_t point) {
plane_t plane;
plane.point = point;
plane.normal = normalizeVector(normal);
return plane;
}
points_t initPoints (const float *x,
const float *y, const float *z,
size_t num_points) {
points_t points;
size_t i;
points.v = (vector_t*) malloc (sizeof(vector_t)*num_points);
if (points.v!=0) {
// printf("created %li\n",points.v);
points.num_points = num_points;
for (i=0; i<num_points; i++) {
points.v[i] = initVector(x[i],y[i],z[i]);
}
}
return points;
}
void freePoints (points_t *points) {
// printf("deleting %li\n",points->v);
if (points!=0) {
if (points->v!=0)
free(points->v);
points->v = 0;
points->num_points = 0;
}
}
vector_t crossProduct (vector_t v1, vector_t v2) {
vector_t cp;
cp.c[0]= v1.c[1]*v2.c[2]-v1.c[2]*v2.c[1];
cp.c[1]=-v1.c[0]*v2.c[2]+v1.c[2]*v2.c[0];
cp.c[2]= v1.c[0]*v2.c[1]-v1.c[1]*v2.c[0];
return cp;
}
float innerProduct (vector_t v1, vector_t v2) {
return v1.c[0]*v2.c[0] + v1.c[1]*v2.c[1] + v1.c[2]*v2.c[2];
}
float distancePointPlane (vector_t point, plane_t plane) {
return innerProduct(point, plane.normal) -
innerProduct(plane.point, plane.normal);
}
vector_t addVectors(vector_t v1, vector_t v2) {
vector_t v3;
v3.c[0]=v1.c[0]+v2.c[0];
v3.c[1]=v1.c[1]+v2.c[1];
v3.c[2]=v1.c[2]+v2.c[2];
return v3;
}
vector_t subtractVectors(vector_t v1, vector_t v2) {
vector_t v3;
v3.c[0]=v1.c[0]-v2.c[0];
v3.c[1]=v1.c[1]-v2.c[1];
v3.c[2]=v1.c[2]-v2.c[2];
return v3;
}
vector_t scaleVector(vector_t v1, float f) {
vector_t v2;
v2.c[0]=f*v1.c[0];
v2.c[1]=f*v1.c[1];
v2.c[2]=f*v1.c[2];
return v2;
}
vector_t averagePoints(points_t points) {
vector_t m = initVector(0.0f, 0.0f, 0.0f);
size_t i;
for (i=0; i<points.num_points; i++)
m=addVectors(points.v[i],m);
m=scaleVector(m,1.0f/((float)points.num_points));
return m;
}
vector_t normalOfPoints(points_t points) {
vector_t m = averagePoints(points);
vector_t n = initVector(0.0f, 0.0f, 0.0f);
size_t i;
for (i=0; i<points.num_points; i++) {
n=addVectors(crossProduct(points.v[i],m),n);
}
return n;
}
plane_t planeFromPoints(points_t points) {
vector_t p=averagePoints(points);
vector_t n=normalOfPoints(points);
plane_t pl=initPlane(n,p);
return pl;
}
void printPlane(plane_t p) {
printf("n=");
printVector(p.normal);
printf(", p=");
printVector(p.point);
}
void printVector(vector_t v) {
printf("[%5.2f,%5.2f,%5.2f], ", v.c[0],v.c[1],v.c[2]);
}
|
