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);
}
Exemple #3
0
    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)
 }
Exemple #9
0
    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);
}