//===========================================================================
cCollisionSpheresNode::cCollisionSpheresNode(std::vector<cTriangle>* a_tris,
cCollisionSpheresSphere *a_parent,
double a_extendedRadius) :
cCollisionSpheresSphere(a_parent)
{
// create cCollisionSpheresTri primitive object for each cTriangle object
Plist primList;
for (unsigned int i=0; i<a_tris->size(); i++)
{
// create cCollisionSpheresPoint primitive object for first point
cVector3d vpos1 = (*a_tris)[i].getVertex0()->getPos();
cVector3d vpos2 = (*a_tris)[i].getVertex1()->getPos();
cVector3d vpos3 = (*a_tris)[i].getVertex2()->getPos();
cCollisionSpheresTri* t = new cCollisionSpheresTri(vpos1,
vpos2,
vpos3,
a_extendedRadius);
t->setOriginal(&(*a_tris)[i]);
// insert new object in primitives list
primList.insert(primList.end(), t);
}
// set parent pointer
m_parent = a_parent;
// create left and right subtrees of this node
ConstructChildren(primList);
}
void erase(int i, Plist& L) { L.erase (L.begin()+i); }
int isfound(pair p, const Plist& L) {
int j = p.f; int k = p.s;
if (L.size()==0) return -1;
for (int i=0; i<L.size(); i++) if (L[i].f==j && L[i].s==k) return i;
return -1;
}
void print_Plist(const Plist& L, str s) {
printf("%s \n",s.c_str());
int n = L.size();
for (int i=0; i<n; i++) printf("%5d : (%d,%d)\n",i,L[i].f,L[i].s);
printf("\n");
}
//===========================================================================
void cCollisionSpheresNode::ConstructChildren(Plist &a_primList)
{
// ensure that there are at least two primitives so that it makes sense
// to split them into left and right subtrees
Plist::iterator primIter;
assert(a_primList.size() >= 2);
// declare and initialize local variables for splitting primitives
Plist leftList, rightList;
double min[3] = {LARGE, LARGE, LARGE};
double max[3] = {-LARGE, -LARGE, -LARGE};
// find minimum and maximum values for each coordinate of primitves' centers
for (primIter = a_primList.begin(); primIter != a_primList.end(); primIter++)
{
cCollisionSpheresGenericShape *cur = *primIter;
const cVector3d ¢er = cur->getCenter();
for (int i = 0; i < 3; i++)
{
if (center.get(i) < min[i])
min[i] = center.get(i);
if (center.get(i) > max[i])
max[i] = center.get(i);
}
}
// find the coordinate index with the largest range (max to min)
int split = 0;
if ((max[1] - min[1]) > (max[split] - min[split]))
split = 1;
if ((max[2] - min[2]) > (max[split] - min[split]))
split = 2;
// sort the primitives according to the coordinate with largest range
cCollisionSpheresGenericShape::m_split = split;
std::sort(a_primList.begin(), a_primList.end());
// put first half in left subtree and second half in right subtree
unsigned int s;
for (s=0; s<a_primList.size()/2; s++)
leftList.insert(leftList.end(), a_primList[s]);
for (s=a_primList.size()/2; s<a_primList.size(); s++)
rightList.insert(rightList.end(), a_primList[s]);
// if the left subtree is empty, transfer one from the right list
if (leftList.size() == 0)
{
leftList.insert(leftList.begin(), *rightList.begin());
rightList.erase(rightList.begin());
}
// create new internal nodes as roots for left and right subtree lists, or
// a leaf node if the subtree list has only one primitive
if (leftList.size() == 1)
m_left = new(g_nextLeafNode++) cCollisionSpheresLeaf(*(leftList.begin()), this);
else
m_left = new(g_nextInternalNode++) cCollisionSpheresNode(leftList, this);
if (rightList.size() == 1)
m_right = new(g_nextLeafNode++) cCollisionSpheresLeaf(*(rightList.begin()), this);
else
m_right = new(g_nextInternalNode++) cCollisionSpheresNode(rightList, this);
// get centers and radii of left and right children
const cVector3d &lc = m_left->m_center;
const cVector3d &rc = m_right->m_center;
double lr = m_left->getRadius();
double rr = m_right->getRadius();
// compute new radius as one-half the sum of the distance between the two
// childrens' centers and the two childrens' radii
double dist = lc.distance(rc);
m_radius = (dist + lr + rr) / 2.0;
// compute new center along line between childrens' centers
if (dist != 0)
{
double lambda = (m_radius - lr) / dist;
m_center.x = lc.x + lambda*(rc.x - lc.x);
m_center.y = lc.y + lambda*(rc.y - lc.y);
m_center.z = lc.z + lambda*(rc.z - lc.z);
}
// if the left and right children have the same center, use this as the
// new center
else
{
m_center = lc;
}
// if one sphere is entirely contained within the other, set this sphere's
// new center and radius equal to those of the larger one
if (lr > dist + rr)
{
m_center = lc;
m_radius = lr;
}
if (rr > dist + lr)
{
m_center = rc;
m_radius = rr;
}
m_radius*=1.001;
}