Build.cpp

Go to the documentation of this file.

/*************************************************************************\

  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


\**************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "PQP.h"
#include "MatVec.h"

// If this is set, build routines will use covariance matrix 
// and mean finding code from RAPID 2.

#define RAPID2_FIT 0

#if RAPID2_FIT

struct moment
{
  PQP_REAL A;  
  PQP_REAL m[3];
  PQP_REAL s[3][3];
};

struct accum
{
  PQP_REAL A;
  PQP_REAL m[3];
  PQP_REAL s[3][3];
};

inline
void
clear_accum(accum &a)
{
  a.m[0] = a.m[1] = a.m[2] = 0.0;
  a.s[0][0] = a.s[0][1] = a.s[0][2] = 0.0;
  a.s[1][0] = a.s[1][1] = a.s[1][2] = 0.0;
  a.s[2][0] = a.s[2][1] = a.s[2][2] = 0.0;
  a.A = 0.0;
}

inline
void
accum_moment(accum &a, moment &b)
{
  a.m[0] += b.m[0] * b.A;
  a.m[1] += b.m[1] * b.A;
  a.m[2] += b.m[2] * b.A;
  
  a.s[0][0] += b.s[0][0];
  a.s[0][1] += b.s[0][1];
  a.s[0][2] += b.s[0][2];
  a.s[1][0] += b.s[1][0];
  a.s[1][1] += b.s[1][1];
  a.s[1][2] += b.s[1][2];
  a.s[2][0] += b.s[2][0];
  a.s[2][1] += b.s[2][1];
  a.s[2][2] += b.s[2][2];

  a.A += b.A;
}

inline
void
mean_from_moment(PQP_REAL M[3], moment &m)
{
  M[0] = m.m[0];
  M[1] = m.m[1];
  M[2] = m.m[2];
}

inline
void
mean_from_accum(PQP_REAL M[3], accum &a)
{
  M[0] = a.m[0] / a.A;
  M[1] = a.m[1] / a.A;
  M[2] = a.m[2] / a.A;
}

inline
void
covariance_from_accum(PQP_REAL C[3][3], accum &a)
{
  int i,j;
  for(i=0; i<3; i++)
    for(j=0; j<3; j++)
      C[i][j] = a.s[i][j] - a.m[i]*a.m[j]/a.A;
}

inline
void
compute_moment(moment &M, PQP_REAL p[3], PQP_REAL q[3], PQP_REAL r[3])
{
  PQP_REAL u[3], v[3], w[3];

  // compute the area of the triangle
  VmV(u, q, p);
  VmV(v, r, p);
  VcrossV(w, u, v);
  M.A = 0.5 * Vlength(w);

  if (M.A == 0.0)
    {
      // This triangle has zero area.  The second order components
      // would be eliminated with the usual formula, so, for the 
      // sake of robustness we use an alternative form.  These are the 
      // centroid and second-order components of the triangle's vertices.

      // centroid
      M.m[0] = (p[0] + q[0] + r[0]) /3;
      M.m[1] = (p[1] + q[1] + r[1]) /3;
      M.m[2] = (p[2] + q[2] + r[2]) /3;

      // second-order components
      M.s[0][0] = (p[0]*p[0] + q[0]*q[0] + r[0]*r[0]);
      M.s[0][1] = (p[0]*p[1] + q[0]*q[1] + r[0]*r[1]);
      M.s[0][2] = (p[0]*p[2] + q[0]*q[2] + r[0]*r[2]);
      M.s[1][1] = (p[1]*p[1] + q[1]*q[1] + r[1]*r[1]);
      M.s[1][2] = (p[1]*p[2] + q[1]*q[2] + r[1]*r[2]);
      M.s[2][2] = (p[2]*p[2] + q[2]*q[2] + r[2]*r[2]);      
      M.s[2][1] = M.s[1][2];
      M.s[1][0] = M.s[0][1];
      M.s[2][0] = M.s[0][2];

      return;
    }

  // get the centroid
  M.m[0] = (p[0] + q[0] + r[0])/3;
  M.m[1] = (p[1] + q[1] + r[1])/3;
  M.m[2] = (p[2] + q[2] + r[2])/3;

  // get the second order components -- note the weighting by the area
  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;
  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;
  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;
  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;
  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;
  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;
  M.s[2][1] = M.s[1][2];
  M.s[1][0] = M.s[0][1];
  M.s[2][0] = M.s[0][2];
}

inline
void
compute_moments(moment *M, Tri *tris, int num_tris)
{
  int i;

  // first collect all the moments, and obtain the area of the 
  // smallest nonzero area triangle.

  PQP_REAL Amin = 0.0;
  int zero = 0;
  int nonzero = 0;
  for(i=0; i<num_tris; i++)
  {
    compute_moment(M[i], 
                   tris[i].p1,
                   tris[i].p2, 
                   tris[i].p3);  
    if (M[i].A == 0.0)
    {
            zero = 1;
    }
    else
    {
            nonzero = 1;
            if (Amin == 0.0) Amin = M[i].A;
            else if (M[i].A < Amin) Amin = M[i].A;
    }
  }

  if (zero)
  {
    fprintf(stderr, "----\n");
    fprintf(stderr, "Warning!  Some triangles have zero area!\n");
    fprintf(stderr, "----\n");

    // if there are any zero area triangles, go back and set their area
  
    // if ALL the triangles have zero area, then set the area thingy
    // to some arbitrary value.
    if (Amin == 0.0) Amin = 1.0;

    for(i=0; i<num_tris; i++)
    {
      if (M[i].A == 0.0) M[i].A = Amin;
    }    
  }
}

#else

PQP_REAL max(PQP_REAL a, PQP_REAL b, PQP_REAL c, PQP_REAL d)
{
  PQP_REAL t = a;
  if (b > t) t = b;
  if (c > t) t = c;
  if (d > t) t = d;
  return t;
}

PQP_REAL min(PQP_REAL a, PQP_REAL b, PQP_REAL c, PQP_REAL d)
{
  PQP_REAL t = a;
  if (b < t) t = b;
  if (c < t) t = c;
  if (d < t) t = d;
  return t;
}

void
get_centroid_triverts(PQP_REAL c[3], Tri *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++)
  {
    PQP_REAL *p1 = tris[i].p1;
    PQP_REAL *p2 = tris[i].p2;
    PQP_REAL *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];      
  }

  PQP_REAL n = (PQP_REAL)(3 * num_tris);

  c[0] /= n;
  c[1] /= n;
  c[2] /= n;
}

void
get_covariance_triverts(PQP_REAL M[3][3], Tri *tris, int num_tris)
{
  int i;
  PQP_REAL S1[3];
  PQP_REAL 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++)
  {
    PQP_REAL *p1 = tris[i].p1;
    PQP_REAL *p2 = tris[i].p2;
    PQP_REAL *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]);
  }

  PQP_REAL n = (PQP_REAL)(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];
}

#endif

// 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

int 
split_tris(Tri *tris, int num_tris, PQP_REAL a[3], PQP_REAL c)
{
  int i;
  int c1 = 0;
  PQP_REAL p[3];
  PQP_REAL x;
  Tri temp;

  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)

int
build_recurse(PQP_Model *m, int bn, int first_tri, int num_tris)
{
  BV *b = m->child(bn);

  // compute a rotation matrix

  PQP_REAL C[3][3], E[3][3], R[3][3], s[3], axis[3], mean[3], coord;

#if RAPID2_FIT
  moment *tri_moment = new moment[num_tris];
  compute_moments(tri_moment, &(m->tris[first_tri]), num_tris);  
  accum acc;
  clear_accum(acc);
  for(int i = 0; i < num_tris; i++) accum_moment(acc, tri_moment[i]);
  delete [] tri_moment;
  covariance_from_accum(C,acc);
#else
  get_covariance_triverts(C,&m->tris[first_tri],num_tris);
#endif

  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->FitToTris(R, &m->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 = m->num_bvs;
    m->num_bvs+=2;

    // choose splitting axis and splitting coord

    McolcV(axis,R,0);

#if RAPID2_FIT
    mean_from_accum(mean,acc);
#else
    get_centroid_triverts(mean,&m->tris[first_tri],num_tris);
#endif
    coord = VdotV(axis, mean);

    // now split

    int num_first_half = split_tris(&m->tris[first_tri], num_tris, 
                                    axis, coord);

    // recursively build the children

    build_recurse(m, m->child(bn)->first_child, first_tri, num_first_half); 
    build_recurse(m, m->child(bn)->first_child + 1,
                  first_tri + num_first_half, num_tris - num_first_half); 
  }
  return PQP_OK;
}

// this descends the hierarchy, converting world-relative 
// transforms to parent-relative transforms

void 
make_parent_relative(PQP_Model *m, int bn,
                     const PQP_REAL parentR[3][3]
#if PQP_BV_TYPE & RSS_TYPE
                     ,const PQP_REAL parentTr[3]
#endif
#if PQP_BV_TYPE & OBB_TYPE
                     ,const PQP_REAL parentTo[3]
#endif
                    )
{
  PQP_REAL Rpc[3][3], Tpc[3];

  if (!m->child(bn)->Leaf())
  {
    // make children parent-relative

    make_parent_relative(m,m->child(bn)->first_child, 
                         m->child(bn)->R
#if PQP_BV_TYPE & RSS_TYPE
                         ,m->child(bn)->Tr
#endif
#if PQP_BV_TYPE & OBB_TYPE
                         ,m->child(bn)->To
#endif
                         );
    make_parent_relative(m,m->child(bn)->first_child+1, 
                         m->child(bn)->R
#if PQP_BV_TYPE & RSS_TYPE
                         ,m->child(bn)->Tr
#endif
#if PQP_BV_TYPE & OBB_TYPE
                         ,m->child(bn)->To
#endif
                         );
  }

  // make self parent relative

  MTxM(Rpc,parentR,m->child(bn)->R);
  McM(m->child(bn)->R,Rpc);
#if PQP_BV_TYPE & RSS_TYPE
  VmV(Tpc,m->child(bn)->Tr,parentTr);
  MTxV(m->child(bn)->Tr,parentR,Tpc);
#endif
#if PQP_BV_TYPE & OBB_TYPE
  VmV(Tpc,m->child(bn)->To,parentTo);
  MTxV(m->child(bn)->To,parentR,Tpc);
#endif

}

int
build_model(PQP_Model *m)
{
  // set num_bvs to 1, the first index for a child bv

  m->num_bvs = 1;

  // build recursively

  build_recurse(m, 0, 0, m->num_tris);

  // change BV orientations from world-relative to parent-relative

  PQP_REAL R[3][3],T[3];
  Midentity(R);
  Videntity(T);

  make_parent_relative(m,0,R
#if PQP_BV_TYPE & RSS_TYPE
                      ,T
#endif
#if PQP_BV_TYPE & OBB_TYPE
                      ,T
#endif
                      );

  return PQP_OK;
}