332 lines
9.6 KiB
C++
332 lines
9.6 KiB
C++
|
#include "precompiled.h"/* Note: this code was adapted from the PQP library;
|
||
|
their copyright notice can be found at the end of this file. */
|
||
|
|
||
|
# include "sr_bv_tree.h"
|
||
|
# include "sr_model.h"
|
||
|
|
||
|
//====================================
|
||
|
//======= SrBvTree::SrBvTree =========
|
||
|
//====================================
|
||
|
|
||
|
SrBvTree::SrBvTree ()
|
||
|
{
|
||
|
_last_tri=0;
|
||
|
}
|
||
|
|
||
|
//=====================================
|
||
|
//======= SrBvTree::~SrBvTree =========
|
||
|
//=====================================
|
||
|
SrBvTree::~SrBvTree ()
|
||
|
{
|
||
|
}
|
||
|
|
||
|
//================================
|
||
|
//======= SrBvTree::init =========
|
||
|
//================================
|
||
|
void SrBvTree::init ()
|
||
|
{
|
||
|
_tris.size(0); _tris.compress();
|
||
|
_bvs.size(0); _bvs.compress();
|
||
|
_last_tri=0;
|
||
|
}
|
||
|
|
||
|
//====================================
|
||
|
//======= SrBvTree::make =========
|
||
|
//====================================
|
||
|
void SrBvTree::make ( const SrModel& m )
|
||
|
{
|
||
|
using namespace SrBvMath;
|
||
|
|
||
|
if ( m.F.size()==0 ) { init(); return; }
|
||
|
|
||
|
// get the geometry of the model:
|
||
|
_tris.size ( m.F.size() );
|
||
|
int i;
|
||
|
for ( i=0; i<_tris.size(); i++ )
|
||
|
{ Vset ( _tris[i].p1, m.V[m.F[i].a] );
|
||
|
Vset ( _tris[i].p2, m.V[m.F[i].b] );
|
||
|
Vset ( _tris[i].p3, m.V[m.F[i].c] );
|
||
|
_tris[i].id = i;
|
||
|
}
|
||
|
_last_tri = &_tris[0];
|
||
|
|
||
|
// create an array of BVs for the model:
|
||
|
_bvs.ensure_capacity ( 2*_tris.size()-1 );
|
||
|
_bvs.size ( 1 );
|
||
|
|
||
|
// build recursively the tree:
|
||
|
_build_recurse ( 0/*node*/, 0/*first*/, _tris.size()/*num*/ );
|
||
|
|
||
|
// change BV orientations from world-relative to parent-relative:
|
||
|
srbvmat R; srbvvec T;
|
||
|
Midentity(R);
|
||
|
Videntity(T);
|
||
|
_make_parent_relative(0,R,T,T);
|
||
|
|
||
|
// make sure the sizes were well defined:
|
||
|
_tris.compress();
|
||
|
_bvs.compress();
|
||
|
}
|
||
|
|
||
|
static void get_centroid_triverts ( srbvvec c, SrBvTri *tris, int num_tris )
|
||
|
{
|
||
|
int i;
|
||
|
|
||
|
c[0] = c[1] = c[2] = 0.0;
|
||
|
|
||
|
// get center of mass
|
||
|
for(i=0; i<num_tris; i++)
|
||
|
{
|
||
|
srbvreal *p1 = tris[i].p1;
|
||
|
srbvreal *p2 = tris[i].p2;
|
||
|
srbvreal *p3 = tris[i].p3;
|
||
|
|
||
|
c[0] += p1[0] + p2[0] + p3[0];
|
||
|
c[1] += p1[1] + p2[1] + p3[1];
|
||
|
c[2] += p1[2] + p2[2] + p3[2];
|
||
|
}
|
||
|
|
||
|
srbvreal n = (srbvreal)(3 * num_tris);
|
||
|
|
||
|
c[0] /= n;
|
||
|
c[1] /= n;
|
||
|
c[2] /= n;
|
||
|
}
|
||
|
|
||
|
static void get_covariance_triverts ( srbvmat M, SrBvTri *tris, int num_tris )
|
||
|
{
|
||
|
int i;
|
||
|
srbvreal S1[3];
|
||
|
srbvreal S2[3][3];
|
||
|
|
||
|
S1[0] = S1[1] = S1[2] = 0.0;
|
||
|
S2[0][0] = S2[1][0] = S2[2][0] = 0.0;
|
||
|
S2[0][1] = S2[1][1] = S2[2][1] = 0.0;
|
||
|
S2[0][2] = S2[1][2] = S2[2][2] = 0.0;
|
||
|
|
||
|
// get center of mass
|
||
|
for(i=0; i<num_tris; i++)
|
||
|
{
|
||
|
srbvreal *p1 = tris[i].p1;
|
||
|
srbvreal *p2 = tris[i].p2;
|
||
|
srbvreal *p3 = tris[i].p3;
|
||
|
|
||
|
S1[0] += p1[0] + p2[0] + p3[0];
|
||
|
S1[1] += p1[1] + p2[1] + p3[1];
|
||
|
S1[2] += p1[2] + p2[2] + p3[2];
|
||
|
|
||
|
S2[0][0] += (p1[0] * p1[0] +
|
||
|
p2[0] * p2[0] +
|
||
|
p3[0] * p3[0]);
|
||
|
S2[1][1] += (p1[1] * p1[1] +
|
||
|
p2[1] * p2[1] +
|
||
|
p3[1] * p3[1]);
|
||
|
S2[2][2] += (p1[2] * p1[2] +
|
||
|
p2[2] * p2[2] +
|
||
|
p3[2] * p3[2]);
|
||
|
S2[0][1] += (p1[0] * p1[1] +
|
||
|
p2[0] * p2[1] +
|
||
|
p3[0] * p3[1]);
|
||
|
S2[0][2] += (p1[0] * p1[2] +
|
||
|
p2[0] * p2[2] +
|
||
|
p3[0] * p3[2]);
|
||
|
S2[1][2] += (p1[1] * p1[2] +
|
||
|
p2[1] * p2[2] +
|
||
|
p3[1] * p3[2]);
|
||
|
}
|
||
|
|
||
|
srbvreal n = (srbvreal)(3 * num_tris);
|
||
|
|
||
|
// now get covariances
|
||
|
|
||
|
M[0][0] = S2[0][0] - S1[0]*S1[0] / n;
|
||
|
M[1][1] = S2[1][1] - S1[1]*S1[1] / n;
|
||
|
M[2][2] = S2[2][2] - S1[2]*S1[2] / n;
|
||
|
M[0][1] = S2[0][1] - S1[0]*S1[1] / n;
|
||
|
M[1][2] = S2[1][2] - S1[1]*S1[2] / n;
|
||
|
M[0][2] = S2[0][2] - S1[0]*S1[2] / n;
|
||
|
M[1][0] = M[0][1];
|
||
|
M[2][0] = M[0][2];
|
||
|
M[2][1] = M[1][2];
|
||
|
}
|
||
|
|
||
|
// given a list of triangles, a splitting axis, and a coordinate on
|
||
|
// that axis, partition the triangles into two groups according to
|
||
|
// where their centroids fall on the axis (under axial projection).
|
||
|
// Returns the number of tris in the first half
|
||
|
static int split_tris ( SrBvTri* tris, int num_tris, srbvvec a, srbvreal c )
|
||
|
{
|
||
|
int i;
|
||
|
int c1 = 0;
|
||
|
srbvvec p;
|
||
|
srbvreal x;
|
||
|
SrBvTri temp;
|
||
|
|
||
|
using namespace SrBvMath;
|
||
|
|
||
|
for(i = 0; i < num_tris; i++)
|
||
|
{
|
||
|
// loop invariant: up to (but not including) index c1 in group 1,
|
||
|
// then up to (but not including) index i in group 2
|
||
|
//
|
||
|
// [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x]
|
||
|
// c1 i
|
||
|
//
|
||
|
VcV(p, tris[i].p1);
|
||
|
VpV(p, p, tris[i].p2);
|
||
|
VpV(p, p, tris[i].p3);
|
||
|
x = VdotV(p, a);
|
||
|
x /= 3.0;
|
||
|
if (x <= c)
|
||
|
{
|
||
|
// group 1
|
||
|
temp = tris[i];
|
||
|
tris[i] = tris[c1];
|
||
|
tris[c1] = temp;
|
||
|
c1++;
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
// group 2 -- do nothing
|
||
|
}
|
||
|
}
|
||
|
|
||
|
// split arbitrarily if one group empty
|
||
|
|
||
|
if ((c1 == 0) || (c1 == num_tris)) c1 = num_tris/2;
|
||
|
|
||
|
return c1;
|
||
|
}
|
||
|
|
||
|
// Fits m->child(bn) to the num_tris triangles starting at first_tri
|
||
|
// Then, if num_tris is greater than one, partitions the tris into two
|
||
|
// sets, and recursively builds two children of m->child(bn)
|
||
|
void SrBvTree::_build_recurse ( int bn, int first_tri, int num_tris )
|
||
|
{
|
||
|
using namespace SrBvMath;
|
||
|
|
||
|
SrBv* b = child ( bn );
|
||
|
|
||
|
// compute a rotation matrix
|
||
|
|
||
|
srbvreal C[3][3], E[3][3], R[3][3], s[3], axis[3], mean[3], coord;
|
||
|
|
||
|
get_covariance_triverts(C,&_tris[first_tri],num_tris);
|
||
|
|
||
|
Meigen(E, s, C);
|
||
|
|
||
|
// place axes of E in order of increasing s
|
||
|
|
||
|
int min, mid, max;
|
||
|
if (s[0] > s[1]) { max = 0; min = 1; }
|
||
|
else { min = 0; max = 1; }
|
||
|
if (s[2] < s[min]) { mid = min; min = 2; }
|
||
|
else if (s[2] > s[max]) { mid = max; max = 2; }
|
||
|
else { mid = 2; }
|
||
|
McolcMcol(R,0,E,max);
|
||
|
McolcMcol(R,1,E,mid);
|
||
|
R[0][2] = E[1][max]*E[2][mid] - E[1][mid]*E[2][max];
|
||
|
R[1][2] = E[0][mid]*E[2][max] - E[0][max]*E[2][mid];
|
||
|
R[2][2] = E[0][max]*E[1][mid] - E[0][mid]*E[1][max];
|
||
|
|
||
|
// fit the BV
|
||
|
b->fit ( R, &_tris[first_tri], num_tris );
|
||
|
|
||
|
if (num_tris == 1)
|
||
|
{
|
||
|
// BV is a leaf BV - first_child will index a triangle
|
||
|
b->first_child = -(first_tri + 1);
|
||
|
}
|
||
|
else if (num_tris > 1)
|
||
|
{
|
||
|
// BV not a leaf - first_child will index a BV
|
||
|
b->first_child = _bvs.size();
|
||
|
_bvs.push();
|
||
|
_bvs.push();
|
||
|
|
||
|
// choose splitting axis and splitting coord
|
||
|
McolcV(axis,R,0);
|
||
|
|
||
|
get_centroid_triverts(mean,&_tris[first_tri],num_tris);
|
||
|
coord = VdotV(axis, mean);
|
||
|
|
||
|
// now split
|
||
|
int num_first_half = split_tris(&_tris[first_tri], num_tris,
|
||
|
axis, coord);
|
||
|
|
||
|
// recursively build the children
|
||
|
_build_recurse( child(bn)->first_child, first_tri, num_first_half);
|
||
|
_build_recurse( child(bn)->first_child + 1,
|
||
|
first_tri + num_first_half, num_tris - num_first_half);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
// this descends the hierarchy, converting world-relative
|
||
|
// transforms to parent-relative transforms
|
||
|
void SrBvTree::_make_parent_relative ( int bn,
|
||
|
const srbvmat parentR,
|
||
|
const srbvvec parentTr,
|
||
|
const srbvvec parentTo )
|
||
|
{
|
||
|
srbvmat Rpc;
|
||
|
srbvvec Tpc;
|
||
|
|
||
|
if ( !child(bn)->leaf() )
|
||
|
{ // make children parent-relative
|
||
|
_make_parent_relative ( child(bn)->first_child,
|
||
|
child(bn)->R,
|
||
|
child(bn)->Tr,
|
||
|
child(bn)->To
|
||
|
);
|
||
|
_make_parent_relative ( child(bn)->first_child+1,
|
||
|
child(bn)->R,
|
||
|
child(bn)->Tr,
|
||
|
child(bn)->To
|
||
|
);
|
||
|
}
|
||
|
|
||
|
// make self parent relative
|
||
|
using namespace SrBvMath;
|
||
|
MTxM(Rpc,parentR,child(bn)->R);
|
||
|
McM(child(bn)->R,Rpc);
|
||
|
VmV(Tpc,child(bn)->Tr,parentTr);
|
||
|
MTxV(child(bn)->Tr,parentR,Tpc);
|
||
|
VmV(Tpc,child(bn)->To,parentTo);
|
||
|
MTxV(child(bn)->To,parentR,Tpc);
|
||
|
}
|
||
|
|
||
|
/*************************************************************************\
|
||
|
Copyright 1999 The University of North Carolina at Chapel Hill.
|
||
|
All Rights Reserved.
|
||
|
|
||
|
Permission to use, copy, modify and distribute this software and its
|
||
|
documentation for educational, research and non-profit purposes, without
|
||
|
fee, and without a written agreement is hereby granted, provided that the
|
||
|
above copyright notice and the following three paragraphs appear in all
|
||
|
copies.
|
||
|
|
||
|
IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
|
||
|
LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
|
||
|
CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
|
||
|
USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
|
||
|
OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
|
||
|
DAMAGES.
|
||
|
|
||
|
THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
|
||
|
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
|
||
|
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE
|
||
|
PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
|
||
|
NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
|
||
|
UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||
|
|
||
|
The authors may be contacted via:
|
||
|
|
||
|
US Mail: S. Gottschalk, E. Larsen
|
||
|
Department of Computer Science
|
||
|
Sitterson Hall, CB #3175
|
||
|
University of N. Carolina
|
||
|
Chapel Hill, NC 27599-3175
|
||
|
Phone: (919)962-1749
|
||
|
EMail: geom@cs.unc.edu
|
||
|
\**************************************************************************/
|