Build.cpp
Go to the documentation of this file.
00001 /*************************************************************************\
00002 
00003   Copyright 1999 The University of North Carolina at Chapel Hill.
00004   All Rights Reserved.
00005 
00006   Permission to use, copy, modify and distribute this software and its
00007   documentation for educational, research and non-profit purposes, without
00008   fee, and without a written agreement is hereby granted, provided that the
00009   above copyright notice and the following three paragraphs appear in all
00010   copies.
00011 
00012   IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
00013   LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
00014   CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
00015   USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
00016   OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
00017   DAMAGES.
00018 
00019   THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
00020   WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
00021   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE
00022   PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
00023   NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
00024   UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
00025 
00026   The authors may be contacted via:
00027 
00028   US Mail:             S. Gottschalk, E. Larsen
00029                        Department of Computer Science
00030                        Sitterson Hall, CB #3175
00031                        University of N. Carolina
00032                        Chapel Hill, NC 27599-3175
00033 
00034   Phone:               (919)962-1749
00035 
00036   EMail:               geom@cs.unc.edu
00037 
00038 
00039 \**************************************************************************/
00040 
00041 #include <stdio.h>
00042 #include <stdlib.h>
00043 #include <string.h>
00044 #include "PQP.h"
00045 #include "MatVec.h"
00046 
00047 // If this is set, build routines will use covariance matrix 
00048 // and mean finding code from RAPID 2.
00049 
00050 #define RAPID2_FIT 0
00051 
00052 #if RAPID2_FIT
00053 
00054 struct moment
00055 {
00056   PQP_REAL A;  
00057   PQP_REAL m[3];
00058   PQP_REAL s[3][3];
00059 };
00060 
00061 struct accum
00062 {
00063   PQP_REAL A;
00064   PQP_REAL m[3];
00065   PQP_REAL s[3][3];
00066 };
00067 
00068 inline
00069 void
00070 clear_accum(accum &a)
00071 {
00072   a.m[0] = a.m[1] = a.m[2] = 0.0;
00073   a.s[0][0] = a.s[0][1] = a.s[0][2] = 0.0;
00074   a.s[1][0] = a.s[1][1] = a.s[1][2] = 0.0;
00075   a.s[2][0] = a.s[2][1] = a.s[2][2] = 0.0;
00076   a.A = 0.0;
00077 }
00078 
00079 inline
00080 void
00081 accum_moment(accum &a, moment &b)
00082 {
00083   a.m[0] += b.m[0] * b.A;
00084   a.m[1] += b.m[1] * b.A;
00085   a.m[2] += b.m[2] * b.A;
00086   
00087   a.s[0][0] += b.s[0][0];
00088   a.s[0][1] += b.s[0][1];
00089   a.s[0][2] += b.s[0][2];
00090   a.s[1][0] += b.s[1][0];
00091   a.s[1][1] += b.s[1][1];
00092   a.s[1][2] += b.s[1][2];
00093   a.s[2][0] += b.s[2][0];
00094   a.s[2][1] += b.s[2][1];
00095   a.s[2][2] += b.s[2][2];
00096 
00097   a.A += b.A;
00098 }
00099 
00100 inline
00101 void
00102 mean_from_moment(PQP_REAL M[3], moment &m)
00103 {
00104   M[0] = m.m[0];
00105   M[1] = m.m[1];
00106   M[2] = m.m[2];
00107 }
00108 
00109 inline
00110 void
00111 mean_from_accum(PQP_REAL M[3], accum &a)
00112 {
00113   M[0] = a.m[0] / a.A;
00114   M[1] = a.m[1] / a.A;
00115   M[2] = a.m[2] / a.A;
00116 }
00117 
00118 inline
00119 void
00120 covariance_from_accum(PQP_REAL C[3][3], accum &a)
00121 {
00122   int i,j;
00123   for(i=0; i<3; i++)
00124     for(j=0; j<3; j++)
00125       C[i][j] = a.s[i][j] - a.m[i]*a.m[j]/a.A;
00126 }
00127 
00128 inline
00129 void
00130 compute_moment(moment &M, PQP_REAL p[3], PQP_REAL q[3], PQP_REAL r[3])
00131 {
00132   PQP_REAL u[3], v[3], w[3];
00133 
00134   // compute the area of the triangle
00135   VmV(u, q, p);
00136   VmV(v, r, p);
00137   VcrossV(w, u, v);
00138   M.A = 0.5 * Vlength(w);
00139 
00140   if (M.A == 0.0)
00141     {
00142       // This triangle has zero area.  The second order components
00143       // would be eliminated with the usual formula, so, for the 
00144       // sake of robustness we use an alternative form.  These are the 
00145       // centroid and second-order components of the triangle's vertices.
00146 
00147       // centroid
00148       M.m[0] = (p[0] + q[0] + r[0]) /3;
00149       M.m[1] = (p[1] + q[1] + r[1]) /3;
00150       M.m[2] = (p[2] + q[2] + r[2]) /3;
00151 
00152       // second-order components
00153       M.s[0][0] = (p[0]*p[0] + q[0]*q[0] + r[0]*r[0]);
00154       M.s[0][1] = (p[0]*p[1] + q[0]*q[1] + r[0]*r[1]);
00155       M.s[0][2] = (p[0]*p[2] + q[0]*q[2] + r[0]*r[2]);
00156       M.s[1][1] = (p[1]*p[1] + q[1]*q[1] + r[1]*r[1]);
00157       M.s[1][2] = (p[1]*p[2] + q[1]*q[2] + r[1]*r[2]);
00158       M.s[2][2] = (p[2]*p[2] + q[2]*q[2] + r[2]*r[2]);      
00159       M.s[2][1] = M.s[1][2];
00160       M.s[1][0] = M.s[0][1];
00161       M.s[2][0] = M.s[0][2];
00162 
00163       return;
00164     }
00165 
00166   // get the centroid
00167   M.m[0] = (p[0] + q[0] + r[0])/3;
00168   M.m[1] = (p[1] + q[1] + r[1])/3;
00169   M.m[2] = (p[2] + q[2] + r[2])/3;
00170 
00171   // get the second order components -- note the weighting by the area
00172   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;
00173   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;
00174   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;
00175   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;
00176   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;
00177   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;
00178   M.s[2][1] = M.s[1][2];
00179   M.s[1][0] = M.s[0][1];
00180   M.s[2][0] = M.s[0][2];
00181 }
00182 
00183 inline
00184 void
00185 compute_moments(moment *M, Tri *tris, int num_tris)
00186 {
00187   int i;
00188 
00189   // first collect all the moments, and obtain the area of the 
00190   // smallest nonzero area triangle.
00191 
00192   PQP_REAL Amin = 0.0;
00193   int zero = 0;
00194   int nonzero = 0;
00195   for(i=0; i<num_tris; i++)
00196   {
00197     compute_moment(M[i], 
00198                    tris[i].p1,
00199                    tris[i].p2, 
00200                    tris[i].p3);  
00201     if (M[i].A == 0.0)
00202     {
00203             zero = 1;
00204     }
00205     else
00206     {
00207             nonzero = 1;
00208             if (Amin == 0.0) Amin = M[i].A;
00209             else if (M[i].A < Amin) Amin = M[i].A;
00210     }
00211   }
00212 
00213   if (zero)
00214   {
00215     fprintf(stderr, "----\n");
00216     fprintf(stderr, "Warning!  Some triangles have zero area!\n");
00217     fprintf(stderr, "----\n");
00218 
00219     // if there are any zero area triangles, go back and set their area
00220   
00221     // if ALL the triangles have zero area, then set the area thingy
00222     // to some arbitrary value.
00223     if (Amin == 0.0) Amin = 1.0;
00224 
00225     for(i=0; i<num_tris; i++)
00226     {
00227       if (M[i].A == 0.0) M[i].A = Amin;
00228     }    
00229   }
00230 }
00231 
00232 #else
00233 
00234 PQP_REAL max(PQP_REAL a, PQP_REAL b, PQP_REAL c, PQP_REAL d)
00235 {
00236   PQP_REAL t = a;
00237   if (b > t) t = b;
00238   if (c > t) t = c;
00239   if (d > t) t = d;
00240   return t;
00241 }
00242 
00243 PQP_REAL min(PQP_REAL a, PQP_REAL b, PQP_REAL c, PQP_REAL d)
00244 {
00245   PQP_REAL t = a;
00246   if (b < t) t = b;
00247   if (c < t) t = c;
00248   if (d < t) t = d;
00249   return t;
00250 }
00251 
00252 void
00253 get_centroid_triverts(PQP_REAL c[3], Tri *tris, int num_tris)
00254 {
00255   int i;
00256 
00257   c[0] = c[1] = c[2] = 0.0;
00258 
00259   // get center of mass
00260   for(i=0; i<num_tris; i++)
00261   {
00262     PQP_REAL *p1 = tris[i].p1;
00263     PQP_REAL *p2 = tris[i].p2;
00264     PQP_REAL *p3 = tris[i].p3;
00265 
00266     c[0] += p1[0] + p2[0] + p3[0];
00267     c[1] += p1[1] + p2[1] + p3[1];
00268     c[2] += p1[2] + p2[2] + p3[2];      
00269   }
00270 
00271   PQP_REAL n = (PQP_REAL)(3 * num_tris);
00272 
00273   c[0] /= n;
00274   c[1] /= n;
00275   c[2] /= n;
00276 }
00277 
00278 void
00279 get_covariance_triverts(PQP_REAL M[3][3], Tri *tris, int num_tris)
00280 {
00281   int i;
00282   PQP_REAL S1[3];
00283   PQP_REAL S2[3][3];
00284 
00285   S1[0] = S1[1] = S1[2] = 0.0;
00286   S2[0][0] = S2[1][0] = S2[2][0] = 0.0;
00287   S2[0][1] = S2[1][1] = S2[2][1] = 0.0;
00288   S2[0][2] = S2[1][2] = S2[2][2] = 0.0;
00289 
00290   // get center of mass
00291   for(i=0; i<num_tris; i++)
00292   {
00293     PQP_REAL *p1 = tris[i].p1;
00294     PQP_REAL *p2 = tris[i].p2;
00295     PQP_REAL *p3 = tris[i].p3;
00296 
00297     S1[0] += p1[0] + p2[0] + p3[0];
00298     S1[1] += p1[1] + p2[1] + p3[1];
00299     S1[2] += p1[2] + p2[2] + p3[2];
00300 
00301     S2[0][0] += (p1[0] * p1[0] +  
00302                  p2[0] * p2[0] +  
00303                  p3[0] * p3[0]);
00304     S2[1][1] += (p1[1] * p1[1] +  
00305                  p2[1] * p2[1] +  
00306                  p3[1] * p3[1]);
00307     S2[2][2] += (p1[2] * p1[2] +  
00308                  p2[2] * p2[2] +  
00309                  p3[2] * p3[2]);
00310     S2[0][1] += (p1[0] * p1[1] +  
00311                  p2[0] * p2[1] +  
00312                  p3[0] * p3[1]);
00313     S2[0][2] += (p1[0] * p1[2] +  
00314                  p2[0] * p2[2] +  
00315                  p3[0] * p3[2]);
00316     S2[1][2] += (p1[1] * p1[2] +  
00317                  p2[1] * p2[2] +  
00318                  p3[1] * p3[2]);
00319   }
00320 
00321   PQP_REAL n = (PQP_REAL)(3 * num_tris);
00322 
00323   // now get covariances
00324 
00325   M[0][0] = S2[0][0] - S1[0]*S1[0] / n;
00326   M[1][1] = S2[1][1] - S1[1]*S1[1] / n;
00327   M[2][2] = S2[2][2] - S1[2]*S1[2] / n;
00328   M[0][1] = S2[0][1] - S1[0]*S1[1] / n;
00329   M[1][2] = S2[1][2] - S1[1]*S1[2] / n;
00330   M[0][2] = S2[0][2] - S1[0]*S1[2] / n;
00331   M[1][0] = M[0][1];
00332   M[2][0] = M[0][2];
00333   M[2][1] = M[1][2];
00334 }
00335 
00336 #endif
00337 
00338 // given a list of triangles, a splitting axis, and a coordinate on
00339 // that axis, partition the triangles into two groups according to
00340 // where their centroids fall on the axis (under axial projection).
00341 // Returns the number of tris in the first half
00342 
00343 int 
00344 split_tris(Tri *tris, int num_tris, PQP_REAL a[3], PQP_REAL c)
00345 {
00346   int i;
00347   int c1 = 0;
00348   PQP_REAL p[3];
00349   PQP_REAL x;
00350   Tri temp;
00351 
00352   for(i = 0; i < num_tris; i++)
00353   {
00354     // loop invariant: up to (but not including) index c1 in group 1,
00355     // then up to (but not including) index i in group 2
00356     //
00357     //  [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x]
00358     //                   c1          i
00359     //
00360     VcV(p, tris[i].p1);
00361     VpV(p, p, tris[i].p2);
00362     VpV(p, p, tris[i].p3);      
00363     x = VdotV(p, a);
00364     x /= 3.0;
00365     if (x <= c)
00366     {
00367             // group 1
00368             temp = tris[i];
00369             tris[i] = tris[c1];
00370             tris[c1] = temp;
00371             c1++;
00372     }
00373     else
00374     {
00375             // group 2 -- do nothing
00376     }
00377   }
00378 
00379   // split arbitrarily if one group empty
00380 
00381   if ((c1 == 0) || (c1 == num_tris)) c1 = num_tris/2;
00382 
00383   return c1;
00384 }
00385 
00386 // Fits m->child(bn) to the num_tris triangles starting at first_tri
00387 // Then, if num_tris is greater than one, partitions the tris into two
00388 // sets, and recursively builds two children of m->child(bn)
00389 
00390 int
00391 build_recurse(PQP_Model *m, int bn, int first_tri, int num_tris)
00392 {
00393   BV *b = m->child(bn);
00394 
00395   // compute a rotation matrix
00396 
00397   PQP_REAL C[3][3], E[3][3], R[3][3], s[3], axis[3], mean[3], coord;
00398 
00399 #if RAPID2_FIT
00400   moment *tri_moment = new moment[num_tris];
00401   compute_moments(tri_moment, &(m->tris[first_tri]), num_tris);  
00402   accum acc;
00403   clear_accum(acc);
00404   for(int i = 0; i < num_tris; i++) accum_moment(acc, tri_moment[i]);
00405   delete [] tri_moment;
00406   covariance_from_accum(C,acc);
00407 #else
00408   get_covariance_triverts(C,&m->tris[first_tri],num_tris);
00409 #endif
00410 
00411   Meigen(E, s, C);
00412 
00413   // place axes of E in order of increasing s
00414 
00415   int min, mid, max;
00416   if (s[0] > s[1]) { max = 0; min = 1; }
00417   else { min = 0; max = 1; }
00418   if (s[2] < s[min]) { mid = min; min = 2; }
00419   else if (s[2] > s[max]) { mid = max; max = 2; }
00420   else { mid = 2; }
00421   McolcMcol(R,0,E,max);
00422   McolcMcol(R,1,E,mid);
00423   R[0][2] = E[1][max]*E[2][mid] - E[1][mid]*E[2][max];
00424   R[1][2] = E[0][mid]*E[2][max] - E[0][max]*E[2][mid];
00425   R[2][2] = E[0][max]*E[1][mid] - E[0][mid]*E[1][max];
00426 
00427   // fit the BV
00428 
00429   b->FitToTris(R, &m->tris[first_tri], num_tris);
00430 
00431   if (num_tris == 1)
00432   {
00433     // BV is a leaf BV - first_child will index a triangle
00434 
00435     b->first_child = -(first_tri + 1);
00436   }
00437   else if (num_tris > 1)
00438   {
00439     // BV not a leaf - first_child will index a BV
00440 
00441     b->first_child = m->num_bvs;
00442     m->num_bvs+=2;
00443 
00444     // choose splitting axis and splitting coord
00445 
00446     McolcV(axis,R,0);
00447 
00448 #if RAPID2_FIT
00449     mean_from_accum(mean,acc);
00450 #else
00451     get_centroid_triverts(mean,&m->tris[first_tri],num_tris);
00452 #endif
00453     coord = VdotV(axis, mean);
00454 
00455     // now split
00456 
00457     int num_first_half = split_tris(&m->tris[first_tri], num_tris, 
00458                                     axis, coord);
00459 
00460     // recursively build the children
00461 
00462     build_recurse(m, m->child(bn)->first_child, first_tri, num_first_half); 
00463     build_recurse(m, m->child(bn)->first_child + 1,
00464                   first_tri + num_first_half, num_tris - num_first_half); 
00465   }
00466   return PQP_OK;
00467 }
00468 
00469 // this descends the hierarchy, converting world-relative 
00470 // transforms to parent-relative transforms
00471 
00472 void 
00473 make_parent_relative(PQP_Model *m, int bn,
00474                      const PQP_REAL parentR[3][3]
00475 #if PQP_BV_TYPE & RSS_TYPE
00476                      ,const PQP_REAL parentTr[3]
00477 #endif
00478 #if PQP_BV_TYPE & OBB_TYPE
00479                      ,const PQP_REAL parentTo[3]
00480 #endif
00481                     )
00482 {
00483   PQP_REAL Rpc[3][3], Tpc[3];
00484 
00485   if (!m->child(bn)->Leaf())
00486   {
00487     // make children parent-relative
00488 
00489     make_parent_relative(m,m->child(bn)->first_child, 
00490                          m->child(bn)->R
00491 #if PQP_BV_TYPE & RSS_TYPE
00492                          ,m->child(bn)->Tr
00493 #endif
00494 #if PQP_BV_TYPE & OBB_TYPE
00495                          ,m->child(bn)->To
00496 #endif
00497                          );
00498     make_parent_relative(m,m->child(bn)->first_child+1, 
00499                          m->child(bn)->R
00500 #if PQP_BV_TYPE & RSS_TYPE
00501                          ,m->child(bn)->Tr
00502 #endif
00503 #if PQP_BV_TYPE & OBB_TYPE
00504                          ,m->child(bn)->To
00505 #endif
00506                          );
00507   }
00508 
00509   // make self parent relative
00510 
00511   MTxM(Rpc,parentR,m->child(bn)->R);
00512   McM(m->child(bn)->R,Rpc);
00513 #if PQP_BV_TYPE & RSS_TYPE
00514   VmV(Tpc,m->child(bn)->Tr,parentTr);
00515   MTxV(m->child(bn)->Tr,parentR,Tpc);
00516 #endif
00517 #if PQP_BV_TYPE & OBB_TYPE
00518   VmV(Tpc,m->child(bn)->To,parentTo);
00519   MTxV(m->child(bn)->To,parentR,Tpc);
00520 #endif
00521 
00522 }
00523 
00524 int
00525 build_model(PQP_Model *m)
00526 {
00527   // set num_bvs to 1, the first index for a child bv
00528 
00529   m->num_bvs = 1;
00530 
00531   // build recursively
00532 
00533   build_recurse(m, 0, 0, m->num_tris);
00534 
00535   // change BV orientations from world-relative to parent-relative
00536 
00537   PQP_REAL R[3][3],T[3];
00538   Midentity(R);
00539   Videntity(T);
00540 
00541   make_parent_relative(m,0,R
00542 #if PQP_BV_TYPE & RSS_TYPE
00543                       ,T
00544 #endif
00545 #if PQP_BV_TYPE & OBB_TYPE
00546                       ,T
00547 #endif
00548                       );
00549 
00550   return PQP_OK;
00551 }


jskeus
Author(s): JSK Alumnis
autogenerated on Fri Aug 28 2015 11:15:08