Vantha/0ad
1
1
Fork 0
forked from 0ad/0ad
Code Pull Requests Activity
b21e798243
0ad/source/dcdt/se/sr_bv_tree_query.cpp
janwas 5bf9bca9ef fix/disable warnings. 
there are too many W4 and "potentially uninitialized", so those are
disabled by 0ad_warning_disable.h.

the silly "int x = strlen" and very dangerous "int x = (void*)p" (and
vice versa) problems are fixed.

This was SVN commit r5526.
2007-12-23 12:18:57 +00:00

841 lines
24 KiB
C++
Raw Blame History

#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 "0ad_warning_disable.h"
# include "sr_bv_tree_query.h"
//====================== SrBvTreeQuery ======================
SrBvTreeQuery::SrBvTreeQuery ()
{
init ();
}
void SrBvTreeQuery::init ()
{
_num_bv_tests = 0;
_num_tri_tests = 0;
_pairs.size(0);
_pairs.compress();
_dist = 0;
_rel_err = _abs_err = 0;
_closer_than_tolerance = false;
_toler = 0;
}
inline void setmv ( srbvmat m, srbvvec v, const SrMat& srm )
{
# define SRM(i) (srbvreal)(srm[i])
// here we pass from column-major to line-major order:
m[0][0]=SRM(0); m[1][0]=SRM(1); m[2][0]=SRM(2);
m[0][1]=SRM(4); m[1][1]=SRM(5); m[2][1]=SRM(6);
m[0][2]=SRM(8); m[1][2]=SRM(9); m[2][2]=SRM(10);
v[0]=SRM(12); v[1]=SRM(13); v[2]=SRM(14);
# undef SRM
}
bool SrBvTreeQuery::collide ( const SrMat& m1, const SrBvTree* t1,
const SrMat& m2, const SrBvTree* t2 )
{
srbvmat R1, R2;
srbvvec T1, T2;
setmv ( R1, T1, m1 );
setmv ( R2, T2, m2 );
collide ( R1, T1, t1, R2, T2, t2, CollideFirstContact );
return _pairs.size()>0? true:false;
}
int SrBvTreeQuery::collide_all ( const SrMat& m1, const SrBvTree* t1,
const SrMat& m2, const SrBvTree* t2 )
{
srbvmat R1, R2;
srbvvec T1, T2;
setmv ( R1, T1, m1 );
setmv ( R2, T2, m2 );
collide ( R1, T1, t1, R2, T2, t2, CollideAllContacts );
return _pairs.size();
}
float SrBvTreeQuery::distance ( const SrMat& m1, SrBvTree* t1,
const SrMat& m2, SrBvTree* t2,
float rel_err, float abs_err )
{
srbvmat R1, R2;
srbvvec T1, T2;
setmv ( R1, T1, m1 );
setmv ( R2, T2, m2 );
_distance ( R1, T1, t1, R2, T2, t2, rel_err, abs_err );
_srp1.set ( _p1 );
_srp2.set ( _p2 );
return distance();
}
bool SrBvTreeQuery::tolerance ( const SrMat& m1, SrBvTree* t1,
const SrMat& m2, SrBvTree* t2,
float tolerance )
{
srbvmat R1, R2;
srbvvec T1, T2;
setmv ( R1, T1, m1 );
setmv ( R2, T2, m2 );
return SrBvTreeQuery::tolerance ( R1, T1, t1, R2, T2, t2, tolerance );
}
float SrBvTreeQuery::greedy_distance ( const SrMat& m1, SrBvTree* t1,
const SrMat& m2, SrBvTree* t2, float delta )
{
srbvmat R1, R2;
srbvvec T1, T2;
setmv ( R1, T1, m1 );
setmv ( R2, T2, m2 );
return SrBvTreeQuery::greedy_distance ( R1, T1, t1, R2, T2, t2, delta );
}
//====================== static functions ======================
inline srbvreal max ( srbvreal a, srbvreal b, srbvreal c )
{
srbvreal t = a;
if (b > t) t = b;
if (c > t) t = c;
return t;
}
inline srbvreal min ( srbvreal a, srbvreal b, srbvreal c )
{
srbvreal t = a;
if (b < t) t = b;
if (c < t) t = c;
return t;
}
static int project6 ( srbvreal *ax,
srbvreal *p1, srbvreal *p2, srbvreal *p3,
srbvreal *q1, srbvreal *q2, srbvreal *q3 )
{
using namespace SrBvMath;
srbvreal P1 = VdotV(ax, p1);
srbvreal P2 = VdotV(ax, p2);
srbvreal P3 = VdotV(ax, p3);
srbvreal Q1 = VdotV(ax, q1);
srbvreal Q2 = VdotV(ax, q2);
srbvreal Q3 = VdotV(ax, q3);
srbvreal mx1 = max(P1, P2, P3);
srbvreal mn1 = min(P1, P2, P3);
srbvreal mx2 = max(Q1, Q2, Q3);
srbvreal mn2 = min(Q1, Q2, Q3);
if (mn1 > mx2) return 0;
if (mn2 > mx1) return 0;
return 1;
}
// very robust triangle intersection test
// uses no divisions
// works on coplanar triangles
static int TriContact(
const srbvreal *P1, const srbvreal *P2, const srbvreal *P3,
const srbvreal *Q1, const srbvreal *Q2, const srbvreal *Q3 )
{
// One triangle is (p1,p2,p3). Other is (q1,q2,q3).
// Edges are (e1,e2,e3) and (f1,f2,f3).
// Normals are n1 and m1
// Outwards are (g1,g2,g3) and (h1,h2,h3).
//
// We assume that the triangle vertices are in the same coordinate system.
//
// First thing we do is establish a new c.s. so that p1 is at (0,0,0).
srbvreal p1[3], p2[3], p3[3];
srbvreal q1[3], q2[3], q3[3];
srbvreal e1[3], e2[3], e3[3];
srbvreal f1[3], f2[3], f3[3];
srbvreal g1[3], g2[3], g3[3];
srbvreal h1[3], h2[3], h3[3];
srbvreal n1[3], m1[3];
srbvreal ef11[3], ef12[3], ef13[3];
srbvreal ef21[3], ef22[3], ef23[3];
srbvreal ef31[3], ef32[3], ef33[3];
p1[0] = P1[0] - P1[0]; p1[1] = P1[1] - P1[1]; p1[2] = P1[2] - P1[2];
p2[0] = P2[0] - P1[0]; p2[1] = P2[1] - P1[1]; p2[2] = P2[2] - P1[2];
p3[0] = P3[0] - P1[0]; p3[1] = P3[1] - P1[1]; p3[2] = P3[2] - P1[2];
q1[0] = Q1[0] - P1[0]; q1[1] = Q1[1] - P1[1]; q1[2] = Q1[2] - P1[2];
q2[0] = Q2[0] - P1[0]; q2[1] = Q2[1] - P1[1]; q2[2] = Q2[2] - P1[2];
q3[0] = Q3[0] - P1[0]; q3[1] = Q3[1] - P1[1]; q3[2] = Q3[2] - P1[2];
e1[0] = p2[0] - p1[0]; e1[1] = p2[1] - p1[1]; e1[2] = p2[2] - p1[2];
e2[0] = p3[0] - p2[0]; e2[1] = p3[1] - p2[1]; e2[2] = p3[2] - p2[2];
e3[0] = p1[0] - p3[0]; e3[1] = p1[1] - p3[1]; e3[2] = p1[2] - p3[2];
f1[0] = q2[0] - q1[0]; f1[1] = q2[1] - q1[1]; f1[2] = q2[2] - q1[2];
f2[0] = q3[0] - q2[0]; f2[1] = q3[1] - q2[1]; f2[2] = q3[2] - q2[2];
f3[0] = q1[0] - q3[0]; f3[1] = q1[1] - q3[1]; f3[2] = q1[2] - q3[2];
using namespace SrBvMath;
VcrossV(n1, e1, e2);
VcrossV(m1, f1, f2);
VcrossV(g1, e1, n1);
VcrossV(g2, e2, n1);
VcrossV(g3, e3, n1);
VcrossV(h1, f1, m1);
VcrossV(h2, f2, m1);
VcrossV(h3, f3, m1);
VcrossV(ef11, e1, f1);
VcrossV(ef12, e1, f2);
VcrossV(ef13, e1, f3);
VcrossV(ef21, e2, f1);
VcrossV(ef22, e2, f2);
VcrossV(ef23, e2, f3);
VcrossV(ef31, e3, f1);
VcrossV(ef32, e3, f2);
VcrossV(ef33, e3, f3);
// now begin the series of tests
if (!project6(n1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(m1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(g1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(g2, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(g3, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(h1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(h2, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(h3, p1, p2, p3, q1, q2, q3)) return 0;
return 1;
}
/**************************/
/******* COLLIDE **********/
/**************************/
void SrBvTreeQuery::collide (
srbvmat R1, srbvvec T1, const SrBvTree* o1,
srbvmat R2, srbvvec T2, const SrBvTree* o2,
CollideFlag flag )
{
// clear the data:
_num_bv_tests = 0;
_num_tri_tests = 0;
_pairs.size(0);
// make sure that the models are built:
if ( o1->nodes()==0 || o2->nodes()==0 ) return;
// Okay, compute what transform [R,T] that takes us from cs1 to cs2.
// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
// First compute the rotation part, then translation part
using namespace SrBvMath;
MTxM(_R,R1,R2);
srbvvec Ttemp;
VmV(Ttemp, T2, T1);
MTxV(_T, R1, Ttemp);
// compute the transform from o1->child(0) to o2->child(0)
srbvmat Rtemp, R;
srbvvec T;
MxM(Rtemp,_R,o2->const_child(0)->R);
MTxM(R,o1->const_child(0)->R,Rtemp);
MxVpV(Ttemp,_R,o2->const_child(0)->To,_T);
VmV(Ttemp,Ttemp,o1->const_child(0)->To);
MTxV(T,o1->const_child(0)->R,Ttemp);
// now start with both top level BVs
_collide_recurse ( R, T, o1, 0, o2, 0, flag );
}
float SrBvTreeQuery::greedy_distance ( srbvmat R1, srbvvec T1, SrBvTree* o1,
srbvmat R2, srbvvec T2, SrBvTree* o2, float delta )
{
using namespace SrBvMath;
MTxM(_R,R1,R2);
double Ttemp[3];
VmV(Ttemp, T2, T1);
MTxV(_T, R1, Ttemp);
_num_bv_tests = 0;
_num_tri_tests = 0;
srbvmat Rtemp, R;
srbvvec T;
MxM(Rtemp,_R,o2->child(0)->R);
MTxM(R,o1->child(0)->R,Rtemp);
MxVpV(Ttemp,_R,o2->child(0)->Tr,_T);
VmV(Ttemp,Ttemp,o1->child(0)->Tr);
MTxV(T,o1->child(0)->R,Ttemp);
// first, see if top-level BVs are separated
_num_bv_tests += 1;
srbvreal d = SrBv::distance(R, T, o1->child(0), o2->child(0));
// if we already found separation don't bother recursing to refine it
if ( float(d) > delta )
return (float)d;
else // if top-level BVs are not separated, recurse
return (float)_greedy_distance_recurse(R,T,o1,0,o2,0,delta);
}
//====================== private methods ======================
inline
srbvreal
TriDistance(srbvreal R[3][3], srbvreal T[3], const SrBvTri *t1, const SrBvTri *t2,
srbvreal p[3], srbvreal q[3])
{
// transform tri 2 into same space as tri 1
srbvreal tri1[3][3], tri2[3][3];
using namespace SrBvMath;
VcV(tri1[0], t1->p1);
VcV(tri1[1], t1->p2);
VcV(tri1[2], t1->p3);
MxVpV(tri2[0], R, t2->p1, T);
MxVpV(tri2[1], R, t2->p2, T);
MxVpV(tri2[2], R, t2->p3, T);
return TriDist(p,q,tri1,tri2);
}
void SrBvTreeQuery::_collide_recurse (
srbvmat R, srbvvec T, // b2 relative to b1
const SrBvTree* o1, int b1,
const SrBvTree* o2, int b2, int flag)
{
// first thing, see if we're overlapping
_num_bv_tests++;
if ( !SrBv::overlap(R,T,o1->const_child(b1), o2->const_child(b2))) return;
// if we are, see if we test triangles next
bool l1 = o1->const_child(b1)->leaf();
bool l2 = o2->const_child(b2)->leaf();
using namespace SrBvMath;
if (l1 && l2)
{
_num_tri_tests++;
// transform the points in b2 into space of b1, then compare
const SrBvTri *t1 = o1->tri(-o1->const_child(b1)->first_child - 1);
const SrBvTri *t2 = o2->tri(-o2->const_child(b2)->first_child - 1);
srbvreal q1[3], q2[3], q3[3];
const srbvreal *p1 = t1->p1;
const srbvreal *p2 = t1->p2;
const srbvreal *p3 = t1->p3;
MxVpV(q1, _R, t2->p1, _T);
MxVpV(q2, _R, t2->p2, _T);
MxVpV(q3, _R, t2->p3, _T);
if (TriContact(p1, p2, p3, q1, q2, q3))
{ // add this to result
_pairs.push() = t1->id;
_pairs.push() = t2->id;
}
return;
}
// we dont, so decide whose children to visit next
srbvreal sz1 = o1->const_child(b1)->size();
srbvreal sz2 = o2->const_child(b2)->size();
srbvreal Rc[3][3],Tc[3],Ttemp[3];
if (l2 || (!l1 && (sz1 > sz2)))
{
int c1 = o1->const_child(b1)->first_child;
int c2 = c1 + 1;
MTxM(Rc,o1->const_child(c1)->R,R);
VmV(Ttemp,T,o1->const_child(c1)->To);
MTxV(Tc,o1->const_child(c1)->R,Ttemp);
_collide_recurse(Rc,Tc,o1,c1,o2,b2,flag);
if ( flag==CollideFirstContact && _pairs.size()>0 ) return;
MTxM(Rc,o1->const_child(c2)->R,R);
VmV(Ttemp,T,o1->const_child(c2)->To);
MTxV(Tc,o1->const_child(c2)->R,Ttemp);
_collide_recurse(Rc,Tc,o1,c2,o2,b2,flag);
}
else
{
int c1 = o2->const_child(b2)->first_child;
int c2 = c1 + 1;
MxM(Rc,R,o2->const_child(c1)->R);
MxVpV(Tc,R,o2->const_child(c1)->To,T);
_collide_recurse(Rc,Tc,o1,b1,o2,c1,flag);
if ( flag==CollideFirstContact && _pairs.size()>0 ) return;
MxM(Rc,R,o2->const_child(c2)->R);
MxVpV(Tc,R,o2->const_child(c2)->To,T);
_collide_recurse(Rc,Tc,o1,b1,o2,c2,flag);
}
}
/***************************/
/******* DISTANCE **********/
/***************************/
void SrBvTreeQuery::_distance ( srbvmat R1, srbvvec T1, SrBvTree* o1,
srbvmat R2, srbvvec T2, SrBvTree* o2,
srbvreal rel_err, srbvreal abs_err )
{
// init data:
_dist = 0;
_num_bv_tests = 0;
_num_tri_tests = 0;
_abs_err = abs_err;
_rel_err = rel_err;
// make sure that the models are built:
if ( o1->nodes()==0 || o2->nodes()==0 ) return;
// Okay, compute what transform [R,T] that takes us from cs2 to cs1.
// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
// First compute the rotation part, then translation part
using namespace SrBvMath;
MTxM(_R,R1,R2);
srbvreal Ttemp[3];
VmV(Ttemp, T2, T1);
MTxV(_T, R1, Ttemp);
// establish initial upper bound using last triangles which
// provided the minimum distance
srbvvec p,q;
_dist = TriDistance(_R,_T,o1->_last_tri,o2->_last_tri,p,q);
VcV(_p1,p);
VcV(_p2,q);
// compute the transform from o1->child(0) to o2->child(0)
srbvreal Rtemp[3][3], R[3][3], T[3];
MxM(Rtemp,_R,o2->const_child(0)->R);
MTxM(R,o1->const_child(0)->R,Rtemp);
MxVpV(Ttemp,_R,o2->const_child(0)->Tr,_T);
VmV(Ttemp,Ttemp,o1->const_child(0)->Tr);
MTxV(T,o1->const_child(0)->R,Ttemp);
_distance_recurse(R,T,o1,0,o2,0);
// res->p2 is in cs 1 ; transform it to cs 2
srbvvec u;
VmV(u, _p2, _T);
MTxV(_p2, _R, u);
}
void SrBvTreeQuery::_distance_recurse (
srbvmat R, srbvvec T, // b2 relative to b1
SrBvTree* o1, int b1,
SrBvTree* o2, int b2 )
{
using namespace SrBvMath;
srbvreal sz1 = o1->const_child(b1)->size();
srbvreal sz2 = o2->const_child(b2)->size();
bool l1 = o1->const_child(b1)->leaf();
bool l2 = o2->const_child(b2)->leaf();
if ( l1 && l2 )
{ // both leaves. Test the triangles beneath them.
_num_tri_tests++;
srbvreal p[3], q[3];
SrBvTri *t1 = &o1->_tris[-o1->child(b1)->first_child - 1];
SrBvTri *t2 = &o2->_tris[-o2->child(b2)->first_child - 1];
srbvreal d = TriDistance(_R,_T,t1,t2,p,q);
if ( d<_dist )
{
_dist = d;
VcV(_p1, p); // p already in c.s. 1
VcV(_p2, q); // q must be transformed
// into c.s. 2 later
o1->_last_tri = t1;
o2->_last_tri = t2;
}
return;
}
// First, perform distance tests on the children. Then traverse
// them recursively, but test the closer pair first, the further
// pair second.
int a1,a2,c1,c2; // new bv tests 'a' and 'c'
srbvreal R1[3][3], T1[3], R2[3][3], T2[3], Ttemp[3];
if (l2 || (!l1 && (sz1 > sz2)))
{
// visit the children of b1
a1 = o1->child(b1)->first_child;
a2 = b2;
c1 = o1->child(b1)->first_child+1;
c2 = b2;
MTxM(R1,o1->const_child(a1)->R,R);
VmV(Ttemp,T,o1->const_child(a1)->Tr);
MTxV(T1,o1->const_child(a1)->R,Ttemp);
MTxM(R2,o1->const_child(c1)->R,R);
VmV(Ttemp,T,o1->const_child(c1)->Tr);
MTxV(T2,o1->const_child(c1)->R,Ttemp);
}
else
{
// visit the children of b2
a1 = b1;
a2 = o2->child(b2)->first_child;
c1 = b1;
c2 = o2->child(b2)->first_child+1;
MxM(R1,R,o2->const_child(a2)->R);
MxVpV(T1,R,o2->const_child(a2)->Tr,T);
MxM(R2,R,o2->const_child(c2)->R);
MxVpV(T2,R,o2->const_child(c2)->Tr,T);
}
_num_bv_tests += 2;
srbvreal d1 = SrBv::distance(R1, T1, o1->const_child(a1), o2->const_child(a2));
srbvreal d2 = SrBv::distance(R2, T2, o1->const_child(c1), o2->const_child(c2));
if (d2 < d1)
{
if ((d2 < (_dist - _abs_err)) ||
(d2*(1 + _rel_err) < _dist))
{
_distance_recurse(R2, T2, o1, c1, o2, c2);
}
if ((d1 < (_dist - _abs_err)) ||
(d1*(1 + _rel_err) < _dist))
{
_distance_recurse(R1, T1, o1, a1, o2, a2);
}
}
else
{
if ((d1 < (_dist - _abs_err)) ||
(d1*(1 + _rel_err) < _dist))
{
_distance_recurse(R1, T1, o1, a1, o2, a2);
}
if ((d2 < (_dist - _abs_err)) ||
(d2*(1 + _rel_err) < _dist))
{
_distance_recurse(R2, T2, o1, c1, o2, c2);
}
}
}
/***************************/
/******* TOLERANCE *********/
/***************************/
void SrBvTreeQuery::_tolerance ( srbvmat R1, srbvvec T1, SrBvTree* o1,
srbvmat R2, srbvvec T2, SrBvTree* o2,
srbvreal tolerance )
{
using namespace SrBvMath;
// init data:
// _dist = 0;
_num_bv_tests = 0;
_num_tri_tests = 0;
if ( tolerance<0 ) tolerance=0.0;
_toler = tolerance;
_closer_than_tolerance = false;
// make sure that the models are built:
if ( o1->nodes()==0 || o2->nodes()==0 ) return;
// Compute the transform [R,T] that takes us from cs2 to cs1.
// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
MTxM(_R,R1,R2);
srbvreal Ttemp[3];
VmV(Ttemp, T2, T1);
MTxV(_T, R1, Ttemp);
// compute the transform from o1->child(0) to o2->child(0)
srbvreal Rtemp[3][3], R[3][3], T[3];
MxM(Rtemp,_R,o2->child(0)->R);
MTxM(R,o1->child(0)->R,Rtemp);
MxVpV(Ttemp,_R,o2->child(0)->Tr,_T);
VmV(Ttemp,Ttemp,o1->child(0)->Tr);
MTxV(T,o1->child(0)->R,Ttemp);
// find a distance lower bound for trivial reject
srbvreal d = SrBv::distance(R, T, o1->child(0), o2->child(0));
if ( d <= _toler )
{ _tolerance_recurse ( R, T, o1, 0, o2, 0 );
}
// res->p2 is in cs 1 ; transform it to cs 2
srbvreal u[3];
VmV(u, _p2, _T);
MTxV(_p2, _R, u);
}
void SrBvTreeQuery::_tolerance_recurse
( srbvmat R, srbvvec T,
SrBvTree* o1, int b1,
SrBvTree* o2, int b2 )
{
using namespace SrBvMath;
srbvreal sz1 = o1->child(b1)->size();
srbvreal sz2 = o2->child(b2)->size();
int l1 = o1->child(b1)->leaf();
int l2 = o2->child(b2)->leaf();
if (l1 && l2)
{
// both leaves - find if tri pair within tolerance
_num_tri_tests++;
srbvreal p[3], q[3];
SrBvTri *t1 = &o1->_tris[-o1->child(b1)->first_child - 1];
SrBvTri *t2 = &o2->_tris[-o2->child(b2)->first_child - 1];
srbvreal d = TriDistance(_R,_T,t1,t2,p,q);
if ( d<=_toler )
{
// triangle pair distance less than tolerance
_closer_than_tolerance = true;
_dist = d;
VcV(_p1, p); // p already in c.s. 1
VcV(_p2, q); // q must be transformed into c.s. 2 later
}
return;
}
int a1,a2,c1,c2; // new bv tests 'a' and 'c'
srbvreal R1[3][3], T1[3], R2[3][3], T2[3], Ttemp[3];
if (l2 || (!l1 && (sz1 > sz2)))
{
// visit the children of b1
a1 = o1->child(b1)->first_child;
a2 = b2;
c1 = o1->child(b1)->first_child+1;
c2 = b2;
MTxM(R1,o1->child(a1)->R,R);
VmV(Ttemp,T,o1->child(a1)->Tr);
MTxV(T1,o1->child(a1)->R,Ttemp);
MTxM(R2,o1->child(c1)->R,R);
VmV(Ttemp,T,o1->child(c1)->Tr);
MTxV(T2,o1->child(c1)->R,Ttemp);
}
else
{
// visit the children of b2
a1 = b1;
a2 = o2->child(b2)->first_child;
c1 = b1;
c2 = o2->child(b2)->first_child+1;
MxM(R1,R,o2->child(a2)->R);
MxVpV(T1,R,o2->child(a2)->Tr,T);
MxM(R2,R,o2->child(c2)->R);
MxVpV(T2,R,o2->child(c2)->Tr,T);
}
_num_bv_tests += 2;
srbvreal d1 = SrBv::distance(R1, T1, o1->child(a1), o2->child(a2));
srbvreal d2 = SrBv::distance(R2, T2, o1->child(c1), o2->child(c2));
if (d2 < d1)
{
if (d2 <= _toler) _tolerance_recurse( R2, T2, o1, c1, o2, c2);
if (_closer_than_tolerance) return;
if (d1 <= _toler) _tolerance_recurse( R1, T1, o1, a1, o2, a2);
}
else
{
if (d1 <= _toler) _tolerance_recurse( R1, T1, o1, a1, o2, a2);
if (_closer_than_tolerance) return;
if (d2 <= _toler) _tolerance_recurse( R2, T2, o1, c1, o2, c2);
}
}
srbvreal SrBvTreeQuery::_greedy_distance_recurse (
srbvmat R, srbvvec T, SrBvTree* o1, int b1,
SrBvTree* o2, int b2, srbvreal delta )
{
using namespace SrBvMath;
int l1 = o1->child(b1)->leaf();
int l2 = o2->child(b2)->leaf();
if (l1 && l2)
{
// both leaves. return their distance
_num_tri_tests++;
double p[3], q[3];
const SrBvTri *t1 = o1->tri(-o1->child(b1)->first_child - 1);
const SrBvTri *t2 = o2->tri(-o2->child(b2)->first_child - 1);
return TriDistance(_R,_T,t1,t2,p,q);
}
// First, perform distance tests on the children. Then traverse
// them recursively, but test the closer pair first, the further
// pair second.
int a1,a2,c1,c2; // new bv tests 'a' and 'c'
double R1[3][3], T1[3], R2[3][3], T2[3], Ttemp[3];
srbvreal sz1 = o1->child(b1)->size();
srbvreal sz2 = o2->child(b2)->size();
if (l2 || (!l1 && (sz1 > sz2)))
{
// visit the children of b1
a1 = o1->child(b1)->first_child;
a2 = b2;
c1 = o1->child(b1)->first_child+1;
c2 = b2;
MTxM(R1,o1->child(a1)->R,R);
VmV(Ttemp,T,o1->child(a1)->Tr);
MTxV(T1,o1->child(a1)->R,Ttemp);
MTxM(R2,o1->child(c1)->R,R);
VmV(Ttemp,T,o1->child(c1)->Tr);
MTxV(T2,o1->child(c1)->R,Ttemp);
}
else
{
// visit the children of b2
a1 = b1;
a2 = o2->child(b2)->first_child;
c1 = b1;
c2 = o2->child(b2)->first_child+1;
MxM(R1,R,o2->child(a2)->R);
MxVpV(T1,R,o2->child(a2)->Tr,T);
MxM(R2,R,o2->child(c2)->R);
MxVpV(T2,R,o2->child(c2)->Tr,T);
}
double d1 = SrBv::distance(R1, T1, o1->child(a1), o2->child(a2));
double d2 = SrBv::distance(R2, T2, o1->child(c1), o2->child(c2));
_num_bv_tests += 2;
// if we already found separation, don't further recurse to refine it
double min_d1d2 = (d1 < d2) ? d1 : d2;
if ( min_d1d2 > delta )
return min_d1d2;
// else recurse
if (d2 < d1) // and thus at least d2 <= delta (d1 maybe too)
{
srbvreal alpha = _greedy_distance_recurse(R2, T2, o1, c1, o2, c2, delta);
if ( alpha > delta ) {
if ( d1 > delta )
return (alpha < d1)? alpha: d1;
srbvreal beta = _greedy_distance_recurse(R1, T1, o1, a1, o2, a2, delta);
if ( beta > delta )
return (alpha < beta)? alpha: beta;
}
return 0;
}
else // at least d1 <= delta (d2 maybe too)
{
srbvreal alpha = _greedy_distance_recurse(R1, T1, o1, a1, o2, a2, delta);
if ( alpha > delta ) {
if ( d2 > delta )
return (alpha < d2)? alpha: d2;
srbvreal beta = _greedy_distance_recurse(R2, T2, o1, c1, o2, c2, delta);
if ( beta > delta )
return (alpha < beta)? alpha: beta;
}
return 0;
}
}
/*************************************************************************\
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
\**************************************************************************/
View Git Blame Copy Permalink