void ExpMapQuaternion::forceOnM_(RefVec out, const ConstRefVec& in)
{
toConstQuat inQuat(in.data());
toQuat outQuat(out.data());
outQuat = inQuat;
out.normalize();
}
void Manifold::forceOnM(RefVec out, const ConstRefVec& in) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(out.size() == representationDim());
mnf_assert(in.size() == representationDim());
return forceOnM_(out, in);
}
virtual void visit(AstVarRef* nodep, AstNUser*) {
if (nodep->lvalue() && !nodep->user2()) {
nodep->user2(true); // mark this ref as visited
AstVar* key = nodep->varp();
VarMap::iterator it = m_lhsmapp->find(key);
if (it == m_lhsmapp->end()) {
// this key does not exist yet, so create it
RefVec* refs = new RefVec();
refs->push_back(nodep);
m_lhsmapp->insert(pair<AstVar*, RefVec*>(key, refs));
} else {
(*it).second->push_back(nodep);
}
nodep->user3p(m_sel); // attach the sel to this varref
}
nodep->iterateChildren(*this);
}
void Manifold::forceOnTxM(RefVec out, const ConstRefVec& in,
const ConstRefVec& x) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(out.size() == tangentDim_);
mnf_assert(x.size() == representationDim());
mnf_assert(in.size() == tangentDim_);
forceOnTxM_(out, in, x);
}
void Manifold::pseudoLog(RefVec out, const ConstRefVec& x,
const ConstRefVec& y) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(out.size() == tangentDim_);
mnf_assert(x.size() == representationDim_);
mnf_assert(y.size() == representationDim_);
pseudoLog_(out, x, y);
}
void Manifold::retractation(RefVec out, const ConstRefVec& x,
const ConstRefVec& v) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(out.size() == representationDim_);
mnf_assert(x.size() == representationDim_);
mnf_assert(v.size() == tangentDim_);
mnf_assert(isInTxM(x, v) && "Wrong tangent vector provided to retractation");
retractation_(out, x, v);
}
void ExpMapQuaternion::setZero_(RefVec out)
{
toQuat(out.data()).setIdentity();
}
void ExpMapQuaternion::retractation_(RefVec out, const ConstRefVec& x, const ConstRefVec& v)
{
OutputType q;
exponential(q,v);
toQuat(out.data()) = (toConstQuat(x.data()))*(toConstQuat(q.data())); //out = x*exp(v)
}
virtual void visit(AstNodeModule* nodep, AstNUser*) {
UINFO(9," MOD "<<nodep<<endl);
m_unique = 0;
VarMap* lhsmapp = new VarMap();
// expand tristate nodes and detect multiple LHS drivers for this module
TristateExpander(nodep, lhsmapp);
// iterate the children to grab any __en signals from subcells
m_modp = nodep;
nodep->iterateChildren(*this);
m_modp = NULL;
// go through each multiple lhs driver & collapse it to a single driver
for (VarMap::iterator nextit, it=lhsmapp->begin(); it != lhsmapp->end(); it=nextit) {
nextit = it; ++nextit;
m_unique = 0;
AstVar* lhsp = (*it).first;
RefVec* refs = (*it).second;
bool isOutput = (lhsp->varType() == AstVarType::OUTPUT) && (nodep->level() > 1); // force termination at top level
if (refs->size() < 2 && isOutput) {
// if only one driver and this is an output, then exit and
// let the driver propagate on its own. If the signals
// terminates at this level, then we need to let the
// undriven state get generated.
lhsmapp->erase(lhsp);
delete refs;
continue;
}
UINFO(9, " Checking " << refs->size() << " drivers for tristates signals on net " << lhsp << endl);
int pull = 0; // initially assume no pull direction
// Now remove and multple lhs signals that do not have __en for
// all possible drivers.
bool complete = true;
int found_one = 0;
for (RefVec::iterator ii=refs->begin(); ii != refs->end(); ++ii) {
AstVarRef* refp = (*ii);
if (!refp->user1p()) { // if no __en signal, then delete the entry
complete = false;
} else {
found_one++;
}
}
if (!complete) {
if (found_one) {
UINFO(9, " Problem mixing tristate and low-Z on " << lhsp << endl);
UINFO(9, " Found " << found_one << " __en signals from of " << refs->size() << " possible drivers" << endl);
// not sure what I should do here other than error that they are mixing low-Z and tristate drivers.
// The other scenerio, and probably more likely, is that they are using a high-Z construct that
// is not supported. Improving the high-Z detection logic will reduce the occurance of this failure.
nodep->v3error("Mixing tristate and low-Z drivers. Perhaps you are using a high-Z construct not supported");
} else {
UINFO(9, " No tristates found on " << lhsp <<endl);
}
lhsmapp->erase(lhsp);
delete refs;
continue;
}
UINFO(9, " TRISTATE LHS DRIVER FOUND:" << lhsp << endl);
AstNode* orp = NULL,* andp = NULL,* undrivenp = NULL,* newenlogicp = NULL;
// loop through the lhs drivers to build the driver resolution logic
for (RefVec::iterator ii=refs->begin(); ii != refs->end(); ++ii) {
AstVarRef* refp = (*ii);
int w = lhsp->width();
int wfill = 0; // width filler when necessary due to sels
AstSel* selp = NULL;
if (refp->user3p()) { // this varref has a sel
selp = (AstSel*) refp->user3p();
w = selp->widthConst();
wfill = lhsp->width() - w;
}
// create a new var for this assignment.
AstVar* enp = (AstVar*)refp->user1p();
AstVar* newlhsp = new AstVar(lhsp->fileline(),
AstVarType::MODULETEMP,
lhsp->name()+"__lhs"+cvtToStr(m_unique++),
AstLogicPacked(), w);
nodep->addStmtp(newlhsp);
// now append this driver to the driver logic.
AstNode* ref1 = new AstVarRef(nodep->fileline(), newlhsp,false);
AstNode* ref2 = new AstVarRef(nodep->fileline(), enp, false);
andp = new AstAnd(nodep->fileline(), ref1, ref2);
AstVar* bitselp = NULL;
if (selp) { // this varref has a sel
int ws = V3Number::log2b(lhsp->width())+1;
bitselp = new AstVar(lhsp->fileline(),
AstVarType::MODULETEMP,
lhsp->name()+"__sel"+cvtToStr(m_unique-1),
AstLogicPacked(), ws);
//
nodep->addStmtp(bitselp);
nodep->addStmtp(new AstAssignW(lhsp->fileline(),
new AstVarRef(lhsp->fileline(), bitselp, true),
selp->lsbp()->cloneTree(false)));
andp = new AstShiftL(lhsp->fileline(),
new AstConcat(lhsp->fileline(), new AstConst(lhsp->fileline(), V3Number(lhsp->fileline(), wfill, 0)), andp),
new AstVarRef(lhsp->fileline(), bitselp, false),
lhsp->width()
);
selp->replaceWith(new AstVarRef(refp->fileline(), newlhsp, true));
pushDeletep(selp); // Setting selp here or deleting immediately
// breaks the t_tri_select test, this probably indicates a problem
} else {
refp->varp(newlhsp); // assign the new var to the varref
refp->name(newlhsp->name());
}
// or this to the others
orp = (!orp) ? andp : new AstOr(nodep->fileline(), orp, andp);
if (isOutput) {
AstNode *en1p = new AstVarRef(nodep->fileline(), enp, false);
if (selp) {
en1p = new AstShiftL(enp->fileline(),
new AstConcat(lhsp->fileline(), new AstConst(lhsp->fileline(), V3Number(lhsp->fileline(), wfill, 0)), en1p),
new AstVarRef(lhsp->fileline(), bitselp, false),
lhsp->width()
);
}
if (!newenlogicp) {
newenlogicp = en1p;
} else {
newenlogicp = new AstOr(nodep->fileline(), newenlogicp, en1p);
}
} else {
if (!undrivenp) {
undrivenp = new AstNot(nodep->fileline(), new AstVarRef(nodep->fileline(), enp, false));
if (selp)
undrivenp = new AstShiftL(enp->fileline(),
new AstConcat(lhsp->fileline(), new AstConst(lhsp->fileline(), V3Number(lhsp->fileline(), wfill, 0)), undrivenp),
new AstVarRef(lhsp->fileline(), bitselp, false),
lhsp->width());
} else {
AstNode *tmp = new AstNot(nodep->fileline(), new AstVarRef(nodep->fileline(), enp, false));
if (selp) {
tmp = new AstShiftL(enp->fileline(),
new AstConcat(lhsp->fileline(), new AstConst(lhsp->fileline(), V3Number(lhsp->fileline(), wfill, 0)), tmp),
new AstVarRef(lhsp->fileline(), bitselp, false),
lhsp->width());
}
undrivenp = new AstAnd(nodep->fileline(), tmp, undrivenp);
}
}
refp->user1p(NULL); // clear the user1p() as we done with it in the VarRef at this point
if (enp->user2()) { // if this net is pulled up/down
int newpull = enp->user2();
if (pull == 0) {
pull = newpull;
} else if (newpull != pull) {
pull = -1; // conflict over the pull direction
}
}
}
if (isOutput) {
AstVar* newenp = new AstVar(lhsp->fileline(),
AstVarType::OUTPUT,
lhsp->name()+"__enout"+cvtToStr(m_unique++),
lhsp);
nodep->addStmtp(newenp);
nodep->addStmtp(new AstAssignW(lhsp->fileline(),
new AstVarRef(lhsp->fileline(), newenp, true),
newenlogicp));
newenp->user2(pull); // put the pull direction in the next __en signal to pass it up
lhsp->user1p(newenp); // put the new __en signal in the var so it can be pushed up the hierarchy.
} else { // this is the level where the signal terminates, we do final conflict resolution here
UINFO(9, " Terminating tristate logic for " << lhsp->name() << endl);
UINFO(9, " Pull direction is " << pull << " where -1=X, 0=Z, 1=low, 2=high." << endl);
// figure out what to drive when no one is driving the bus
V3Number num(nodep->fileline(), lhsp->width());
if (pull==0) {
num.setAllBitsZ();
} else if (pull==1) {
num.setAllBits0();
} else if (pull==2) {
num.setAllBits1();
} else {
num.setAllBitsX();
}
undrivenp = new AstAnd(nodep->fileline(), undrivenp,
new AstConst(nodep->fileline(), num));
orp = new AstOr(nodep->fileline(), orp, undrivenp);
}
nodep->addStmtp(new AstAssignW(lhsp->fileline(),
new AstVarRef(lhsp->fileline(), lhsp, true), orp));
// delete the map and vector list now that we have collapsed it.
lhsmapp->erase(lhsp);
delete refs;
}
delete lhsmapp; // delete the map now that we are done
nodep->user1p(NULL);
}
void Manifold::createRandomPoint(RefVec out, double coeff) const
{
mnf_assert(out.size() == representationDim_ &&
"wrong dimension in Manifold::createRandomPoint");
createRandomPoint_(out, coeff);
}
void Manifold::getTrustMagnitude(RefVec out) const
{
mnf_assert(out.size() == tangentDim_);
getTrustMagnitude_(out);
}
void Manifold::limitMap(RefVec out) const
{
mnf_assert(out.size() == tangentDim_);
limitMap_(out);
}
void Manifold::setZero(RefVec out) const
{
mnf_assert(isValid() || seeMessageAbove());
mnf_assert(out.size() == representationDim_);
setZero_(out);
}
void Manifold::pseudoLog0(RefVec out, const ConstRefVec& x) const
{
mnf_assert(out.size() == tangentDim_);
mnf_assert(x.size() == representationDim_);
pseudoLog0_(out, x);
}