1 #ifndef MOMENTS_H
2 #define MOMENTS_H
3
4 #include "matvec.H"
5 #include "obb.H"
6
7 struct moment
8 {
9 double A;
10 double m[3];
11 double s[3][3];
12 };
13
14 struct accum
15 {
16 double A;
17 double m[3];
18 double s[3][3];
19 };
20
21 inline
22 void
print_moment(moment & M)23 print_moment(moment &M)
24 {
25 fprintf(stderr, "A: %g, m: %g %g %g, s: %g %g %g %g %g %g\n",
26 M.A,
27 M.m[0], M.m[1], M.m[2],
28 M.s[0][0], M.s[0][1], M.s[0][2], M.s[1][1], M.s[1][2], M.s[2][2]);
29 }
30
31
32 inline
33 void
clear_accum(accum & a)34 clear_accum(accum &a)
35 {
36 a.m[0] = a.m[1] = a.m[2] = 0.0;
37 a.s[0][0] = a.s[0][1] = a.s[0][2] = 0.0;
38 a.s[1][0] = a.s[1][1] = a.s[1][2] = 0.0;
39 a.s[2][0] = a.s[2][1] = a.s[2][2] = 0.0;
40 a.A = 0.0;
41 }
42
43 inline
44 void
accum_moment(accum & a,moment & b)45 accum_moment(accum &a, moment &b)
46 {
47 a.m[0] += b.m[0] * b.A;
48 a.m[1] += b.m[1] * b.A;
49 a.m[2] += b.m[2] * b.A;
50
51 a.s[0][0] += b.s[0][0];
52 a.s[0][1] += b.s[0][1];
53 a.s[0][2] += b.s[0][2];
54 a.s[1][0] += b.s[1][0];
55 a.s[1][1] += b.s[1][1];
56 a.s[1][2] += b.s[1][2];
57 a.s[2][0] += b.s[2][0];
58 a.s[2][1] += b.s[2][1];
59 a.s[2][2] += b.s[2][2];
60
61 a.A += b.A;
62 }
63
64 inline
65 void
mean_from_moment(double M[3],moment & m)66 mean_from_moment(double M[3], moment &m)
67 {
68 M[0] = m.m[0];
69 M[1] = m.m[1];
70 M[2] = m.m[2];
71 }
72
73 inline
74 void
mean_from_accum(double M[3],accum & a)75 mean_from_accum(double M[3], accum &a)
76 {
77 M[0] = a.m[0] / a.A;
78 M[1] = a.m[1] / a.A;
79 M[2] = a.m[2] / a.A;
80 }
81
82 inline
83 void
covariance_from_accum(double C[3][3],accum & a)84 covariance_from_accum(double C[3][3], accum &a)
85 {
86 int i,j;
87 for(i=0; i<3; i++)
88 for(j=0; j<3; j++)
89 C[i][j] = a.s[i][j] - a.m[i]*a.m[j]/a.A;
90 }
91
92
93
94 inline
95 void
compute_moment(moment & M,double p[3],double q[3],double r[3])96 compute_moment(moment &M, double p[3], double q[3], double r[3])
97 {
98 double u[3], v[3], w[3];
99
100 // compute the area of the triangle
101 VmV(u, q, p);
102 VmV(v, r, p);
103 VcrossV(w, u, v);
104 M.A = 0.5 * Vlength(w);
105
106 if (M.A == 0.0)
107 {
108 // This triangle has zero area. The second order components
109 // would be eliminated with the usual formula, so, for the
110 // sake of robustness we use an alternative form. These are the
111 // centroid and second-order components of the triangle's vertices.
112
113 // centroid
114 M.m[0] = (p[0] + q[0] + r[0]) /3;
115 M.m[1] = (p[1] + q[1] + r[1]) /3;
116 M.m[2] = (p[2] + q[2] + r[2]) /3;
117
118 // second-order components
119 M.s[0][0] = (p[0]*p[0] + q[0]*q[0] + r[0]*r[0]);
120 M.s[0][1] = (p[0]*p[1] + q[0]*q[1] + r[0]*r[1]);
121 M.s[0][2] = (p[0]*p[2] + q[0]*q[2] + r[0]*r[2]);
122 M.s[1][1] = (p[1]*p[1] + q[1]*q[1] + r[1]*r[1]);
123 M.s[1][2] = (p[1]*p[2] + q[1]*q[2] + r[1]*r[2]);
124 M.s[2][2] = (p[2]*p[2] + q[2]*q[2] + r[2]*r[2]);
125 M.s[2][1] = M.s[1][2];
126 M.s[1][0] = M.s[0][1];
127 M.s[2][0] = M.s[0][2];
128
129 return;
130 }
131
132 // get the centroid
133 M.m[0] = (p[0] + q[0] + r[0])/3;
134 M.m[1] = (p[1] + q[1] + r[1])/3;
135 M.m[2] = (p[2] + q[2] + r[2])/3;
136
137 // get the second order components -- note the weighting by the area
138 M.s[0][0] = M.A*(9*M.m[0]*M.m[0]+p[0]*p[0]+q[0]*q[0]+r[0]*r[0])/12;
139 M.s[0][1] = M.A*(9*M.m[0]*M.m[1]+p[0]*p[1]+q[0]*q[1]+r[0]*r[1])/12;
140 M.s[1][1] = M.A*(9*M.m[1]*M.m[1]+p[1]*p[1]+q[1]*q[1]+r[1]*r[1])/12;
141 M.s[0][2] = M.A*(9*M.m[0]*M.m[2]+p[0]*p[2]+q[0]*q[2]+r[0]*r[2])/12;
142 M.s[1][2] = M.A*(9*M.m[1]*M.m[2]+p[1]*p[2]+q[1]*q[2]+r[1]*r[2])/12;
143 M.s[2][2] = M.A*(9*M.m[2]*M.m[2]+p[2]*p[2]+q[2]*q[2]+r[2]*r[2])/12;
144 M.s[2][1] = M.s[1][2];
145 M.s[1][0] = M.s[0][1];
146 M.s[2][0] = M.s[0][2];
147 }
148
149 inline
150 void
compute_moments(moment * M,tri * tris,int num_tris)151 compute_moments(moment *M, tri *tris, int num_tris)
152 {
153 int i;
154
155 // first collect all the moments, and obtain the area of the
156 // smallest nonzero area triangle.
157
158 double Amin = 0.0;
159 int zero = 0;
160 int nonzero = 0;
161 for(i=0; i<num_tris; i++)
162 {
163 compute_moment(M[i],
164 tris[i].p1,
165 tris[i].p2,
166 tris[i].p3);
167 if (M[i].A == 0.0)
168 {
169 zero = 1;
170 }
171 else
172 {
173 nonzero = 1;
174 if (Amin == 0.0) Amin = M[i].A;
175 else if (M[i].A < Amin) Amin = M[i].A;
176 }
177 }
178
179 if (zero)
180 {
181 fprintf(stderr, "----\n");
182 fprintf(stderr, "Warning! Some triangles have zero area!\n");
183 fprintf(stderr, "----\n");
184
185 // if there are any zero area triangles, go back and set their area
186
187 // if ALL the triangles have zero area, then set the area thingy
188 // to some arbitrary value.
189 if (Amin == 0.0) Amin = 1.0;
190
191 for(i=0; i<num_tris; i++)
192 {
193 if (M[i].A == 0.0) M[i].A = Amin;
194 }
195
196 }
197 }
198
199 #endif
200