void DominatorTree::debugPrint()
{
for (int i = 0; i < count; ++i) {
INFO("SEMI(%i) = %i\n", i, SEMI(i));
INFO("ANCESTOR(%i) = %i\n", i, ANCESTOR(i));
INFO("PARENT(%i) = %i\n", i, PARENT(i));
INFO("LABEL(%i) = %i\n", i, LABEL(i));
INFO("DOM(%i) = %i\n", i, DOM(i));
}
}
std::unique_ptr< NHomMarkedAbelianGroup >
NHomGroupPresentation::markedAbelianisation() const
{
std::unique_ptr<NMarkedAbelianGroup> DOM( domain_->markedAbelianisation() );
std::unique_ptr<NMarkedAbelianGroup> RAN( range_->markedAbelianisation() );
NMatrixInt ccMat( RAN->rankCC(), DOM->rankCC() );
for (unsigned long j=0; j<ccMat.columns(); j++)
{
NGroupExpression COLj( evaluate(j) );
for (unsigned long i=0; i<COLj.countTerms(); i++)
ccMat.entry( COLj.generator(i), j ) += COLj.exponent(i);
}
return std::unique_ptr<NHomMarkedAbelianGroup>(
new NHomMarkedAbelianGroup( *DOM, *RAN, ccMat ) );
}
void DominatorTree::build()
{
DLList *bucket = new DLList[count];
Node *nv, *nw;
int p, u, v, w;
buildDFS(cfg->getRoot());
for (w = count - 1; w >= 1; --w) {
nw = vert[w];
assert(nw->tag == w);
for (Graph::EdgeIterator ei = nw->incident(); !ei.end(); ei.next()) {
nv = ei.getNode();
v = nv->tag;
u = eval(v);
if (SEMI(u) < SEMI(w))
SEMI(w) = SEMI(u);
}
p = PARENT(w);
bucket[SEMI(w)].insert(nw);
link(p, w);
for (DLList::Iterator it = bucket[p].iterator(); !it.end(); it.erase()) {
v = reinterpret_cast<Node *>(it.get())->tag;
u = eval(v);
DOM(v) = (SEMI(u) < SEMI(v)) ? u : p;
}
}
for (w = 1; w < count; ++w) {
if (DOM(w) != SEMI(w))
DOM(w) = DOM(DOM(w));
}
DOM(0) = 0;
insert(&BasicBlock::get(cfg->getRoot())->dom);
do {
p = 0;
for (v = 1; v < count; ++v) {
nw = &BasicBlock::get(vert[DOM(v)])->dom;;
nv = &BasicBlock::get(vert[v])->dom;
if (nw->getGraph() && !nv->getGraph()) {
++p;
nw->attach(nv, Graph::Edge::TREE);
}
}
} while (p);
delete[] bucket;
}
VL_EXPORT
void vl_irodrigues(double* om_pt, double* dom_pt, const double* R_pt)
{
/*
tr R - 1 1 [ R32 - R23 ]
th = cos^{-1} --------, r = ------ [ R13 - R31 ], w = th r.
2 2 sth [ R12 - R21 ]
sth = sin(th)
dw th*cth-sth dw th [di3 dj2 - di2 dj3]
---- = ---------- r, ---- = ----- [di1 dj3 - di3 dj1].
dRii 2 sth^2 dRij 2 sth [di1 dj2 - di2 dj1]
trace(A) < -1 only for small num. errors.
*/
#define OM(i) om_pt[(i)]
#define DOM(i,j) dom_pt[(i)+3*(j)]
#define R(i,j) R_pt[(i)+3*(j)]
#define W(i,j) W_pt[(i)+3*(j)]
const double small = 1e-6 ;
double th = acos
(0.5*(VL_MAX(R(0,0)+R(1,1)+R(2,2),-1.0) - 1.0)) ;
double sth = sin(th) ;
double cth = cos(th) ;
if(fabs(sth) < small && cth < 0) {
/*
we have this singularity when the rotation is about pi (or -pi)
we use the fact that in this case
hat( sqrt(1-cth) * r )^2 = W = (0.5*(R+R') - eye(3))
which gives
(1-cth) rx^2 = 0.5 * (W(1,1)-W(2,2)-W(3,3))
(1-cth) ry^2 = 0.5 * (W(2,2)-W(3,3)-W(1,1))
(1-cth) rz^2 = 0.5 * (W(3,3)-W(1,1)-W(2,2))
*/
double W_pt [9], x, y, z ;
W_pt[0] = 0.5*( R(0,0) + R(0,0) ) - 1.0 ;
W_pt[1] = 0.5*( R(1,0) + R(0,1) ) ;
W_pt[2] = 0.5*( R(2,0) + R(0,2) );
W_pt[3] = 0.5*( R(0,1) + R(1,0) );
W_pt[4] = 0.5*( R(1,1) + R(1,1) ) - 1.0;
W_pt[5] = 0.5*( R(2,1) + R(1,2) );
W_pt[6] = 0.5*( R(0,2) + R(2,0) ) ;
W_pt[7] = 0.5*( R(1,2) + R(2,1) ) ;
W_pt[8] = 0.5*( R(2,2) + R(2,2) ) - 1.0 ;
/* these are only absolute values */
x = sqrt( 0.5 * (W(0,0)-W(1,1)-W(2,2)) ) ;
y = sqrt( 0.5 * (W(1,1)-W(2,2)-W(0,0)) ) ;
z = sqrt( 0.5 * (W(2,2)-W(0,0)-W(1,1)) ) ;
/* set the biggest component to + and use the element of the
** matrix W to determine the sign of the other components
** then the solution is either (x,y,z) or its opposite */
if( x >= y && x >= z ) {
y = (W(1,0) >=0) ? y : -y ;
z = (W(2,0) >=0) ? z : -z ;
} else if( y >= x && y >= z ) {
z = (W(2,1) >=0) ? z : -z ;
x = (W(1,0) >=0) ? x : -x ;
} else {
x = (W(2,0) >=0) ? x : -x ;
y = (W(2,1) >=0) ? y : -y ;
}
/* we are left to chose between (x,y,z) and (-x,-y,-z)
** unfortunately we cannot (as the rotation is too close to pi) and
** we just keep what we have. */
{
double scale = th / sqrt( 1 - cth ) ;
OM(0) = scale * x ;
OM(1) = scale * y ;
OM(2) = scale * z ;
if( dom_pt ) {
int k ;
for(k=0; k<3*9; ++k)
dom_pt [k] = VL_NAN_D ;
}
return ;
}
} else {
double a = (fabs(sth) < small) ? 1 : th/sin(th) ;
double b ;
OM(0) = 0.5*a*(R(2,1) - R(1,2)) ;
OM(1) = 0.5*a*(R(0,2) - R(2,0)) ;
OM(2) = 0.5*a*(R(1,0) - R(0,1)) ;
if( dom_pt ) {
if( fabs(sth) < small ) {
a = 0.5 ;
b = 0 ;
} else {
a = th/(2*sth) ;
b = (th*cth - sth)/(2*sth*sth)/th ;
}
DOM(0,0) = b*OM(0) ;
DOM(1,0) = b*OM(1) ;
DOM(2,0) = b*OM(2) ;
DOM(0,1) = 0 ;
DOM(1,1) = 0 ;
DOM(2,1) = a ;
DOM(0,2) = 0 ;
DOM(1,2) = -a ;
DOM(2,2) = 0 ;
DOM(0,3) = 0 ;
DOM(1,3) = 0 ;
DOM(2,3) = -a ;
DOM(0,4) = b*OM(0) ;
DOM(1,4) = b*OM(1) ;
DOM(2,4) = b*OM(2) ;
DOM(0,5) = a ;
DOM(1,5) = 0 ;
DOM(2,5) = 0 ;
DOM(0,6) = 0 ;
DOM(1,6) = a ;
DOM(2,6) = 0 ;
DOM(0,7) = -a ;
DOM(1,7) = 0 ;
DOM(2,7) = 0 ;
DOM(0,8) = b*OM(0) ;
DOM(1,8) = b*OM(1) ;
DOM(2,8) = b*OM(2) ;
}
}
#undef OM
#undef DOM
#undef R
#undef W
}
