void CSetDlgColors::ColorSetEdit(HWND hWnd2, WORD c) { _ASSERTE(hWnd2!=NULL); // Hook ctrl procedure if (!gColorBoxMap.Initialized()) gColorBoxMap.Init(128); HWND hBox = GetDlgItem(hWnd2, c); if (!gColorBoxMap.Get(hBox, NULL)) { WNDPROC pfnOld = (WNDPROC)SetWindowLongPtr(hBox, GWLP_WNDPROC, (LONG_PTR)ColorBoxProc); gColorBoxMap.Set(hBox, pfnOld); } // text box ID WORD tc = (tc0-c0) + c; // Well, 11 chars are enough for "255 255 255" // But sometimes it is interesting to copy/paste/cut when editing palettes, so x2 SendDlgItemMessage(hWnd2, tc, EM_SETLIMITTEXT, 23, 0); COLORREF cr = 0; GetColorById(c, &cr); wchar_t temp[16]; switch (gpSetCls->m_ColorFormat) { case CSettings::eRgbHex: _wsprintf(temp, SKIPLEN(countof(temp)) L"#%02x%02x%02x", getR(cr), getG(cr), getB(cr)); break; case CSettings::eBgrHex: _wsprintf(temp, SKIPLEN(countof(temp)) L"0x%02x%02x%02x", getB(cr), getG(cr), getR(cr)); break; default: _wsprintf(temp, SKIPLEN(countof(temp)) L"%i %i %i", getR(cr), getG(cr), getB(cr)); } SetDlgItemText(hWnd2, tc, temp); }
/* bool Dstar::replan() * -------------------------- * Updates the costs for all cells and computes the shortest path to * goal. Returns true if a path is found, false otherwise. The path is * computed by doing a greedy search over the cost+g values in each * cells. In order to get around the problem of the robot taking a * path that is near a 45 degree angle to goal we break ties based on * the metric euclidean(state, goal) + euclidean(state,start). */ bool Dstar::replan() { path.clear(); int res = computeShortestPath(); //printf("res: %d ols: %d ohs: %d tk: [%f %f] sk: [%f %f] sgr: (%f,%f)\n",res,openList.size(),openHash.size(),openList.top().k.first,openList.top().k.second, s_start.k.first, s_start.k.second,getRHS(s_start),getG(s_start)); if (res < 0) { fprintf(stderr, "NO PATH TO GOAL\n"); return false; } list<state> n; list<state>::iterator i; state cur = s_start; if (isinf(getG(s_start))) { fprintf(stderr, "NO PATH TO GOAL\n"); return false; } while(cur != s_goal) { path.push_back(cur); getSucc(cur, n); if (n.empty()) { fprintf(stderr, "NO PATH TO GOAL\n"); return false; } double cmin = INFINITY; double tmin; state smin; for (i=n.begin(); i!=n.end(); i++) { //if (occupied(*i)) continue; double val = cost(cur,*i); double val2 = trueDist(*i,s_goal) + trueDist(s_start,*i); // (Euclidean) cost to goal + cost to pred val += getG(*i); if (close(val,cmin)) { if (tmin > val2) { tmin = val2; cmin = val; smin = *i; } } else if (val < cmin) { tmin = val2; cmin = val; smin = *i; } } n.clear(); cur = smin; } path.push_back(s_goal); return true; }
/* int Dstar::computeShortestPath() * -------------------------- * As per [S. Koenig, 2002] except for 2 main modifications: * 1. We stop planning after a number of steps, 'maxsteps' we do this * because this algorithm can plan forever if the start is * surrounded by obstacles. * 2. We lazily remove states from the open list so we never have to * iterate through it. */ int Dstar::computeShortestPath() { list<state> s; list<state>::iterator i; if (openList.empty()) return 1; int k=0; while ((!openList.empty()) && (openList.top() < (s_start = calculateKey(s_start))) || (getRHS(s_start) != getG(s_start))) { if (k++ > maxSteps) { fprintf(stderr, "At maxsteps\n"); return -1; } state u; bool test = (getRHS(s_start) != getG(s_start)); // lazy remove while(1) { if (openList.empty()) return 1; u = openList.top(); openList.pop(); if (!isValid(u)) continue; if (!(u < s_start) && (!test)) return 2; break; } ds_oh::iterator cur = openHash.find(u); openHash.erase(cur); state k_old = u; if (k_old < calculateKey(u)) { // u is out of date insert(u); } else if (getG(u) > getRHS(u)) { // needs update (got better) setG(u,getRHS(u)); getPred(u,s); for (i=s.begin();i != s.end(); i++) { updateVertex(*i); } } else { // g <= rhs, state has got worse setG(u,INFINITY); getPred(u,s); for (i=s.begin();i != s.end(); i++) { updateVertex(*i); } updateVertex(u); } } return 0; }
bool WindowBitmap::rgbEqual(int x1, int y1, int x2, int y2) { int r1 = getR(x1, y1); int g1 = getG(x1, y1); int b1 = getB(x1, y1); int r2 = getR(x2, y2); int g2 = getG(x2, y2); int b2 = getB(x2, y2); return (r1 == r2) && (g1 == g2) && (b1 == b2); }
void MCNeuronSim::setupSingleNeuronParms(int grpRowId, int neurId, bool coupledComp){ for(unsigned int c = 0; c < compCount; c++) // each neuron has compCount compartments { network->setIzhikevichParameter(excGroup[grpRowId][c], neurId, "C", getCm(neurId, c)); network->setIzhikevichParameter(excGroup[grpRowId][c], neurId, "k", getK(neurId, c)); network->setIzhikevichParameter(excGroup[grpRowId][c], neurId, "vr", getVr(neurId)); network->setIzhikevichParameter(excGroup[grpRowId][c], neurId, "vt", getVt(neurId, c)); network->setIzhikevichParameter(excGroup[grpRowId][c], neurId, "a", getA(neurId, c)); network->setIzhikevichParameter(excGroup[grpRowId][c], neurId, "b", getB(neurId, c)); network->setIzhikevichParameter(excGroup[grpRowId][c], neurId, "vpeak", getVpeak(neurId, c)); network->setIzhikevichParameter(excGroup[grpRowId][c], neurId, "c", getVmin(neurId, c)); network->setIzhikevichParameter(excGroup[grpRowId][c], neurId, "d", getD(neurId, c)); if(coupledComp){ if(c>0){ double G = getG(neurId, c); //parameters[neurId][G_idx[c-1]]; double P = getP(neurId, c);//parameters[neurId][P_idx[c-1]]; float fwd = G * P; float bwd = G * (1-P); /* * generally, fwd is carlsim 'down', bwd is carlsim 'up' for the purpose of coupling constant assignment, but, * when there is a dendrite 'below' soma: ****cases 3c2 and 4c2*** * up and down are reversed. */ if(compCount>2 && c==1 && connLayout[c]==connLayout[c+1]){ //meaning 2 dendrites (dend 1 and dend2 ) connecting to the same point network->setCouplingConstant(excGroup[grpRowId][connLayout[c]], neurId, "down", bwd); network->setCouplingConstant(excGroup[grpRowId][c], neurId, "up", fwd); }else{ network->setCouplingConstant(excGroup[grpRowId][c], neurId, "down", fwd); network->setCouplingConstant(excGroup[grpRowId][connLayout[c]], neurId, "up", bwd); } } } } }
const Matrix& PFEMElement2DBubble::getDamp() { // resize K int ndf = this->getNumDOF(); K.resize(ndf, ndf); K.Zero(); Vector G(6); getG(G); Matrix L(3,3); getL(L); // other matrices for(int a=0; a<3; a++) { for(int b=0; b<3; b++) { K(numDOFs(2*a+1), numDOFs(2*b)) = G(2*b); // GxT K(numDOFs(2*a+1), numDOFs(2*b)+1) = G(2*b+1); // GyT K(numDOFs(2*a), numDOFs(2*b+1)) = -G(2*a); // -Gx K(numDOFs(2*a)+1, numDOFs(2*b+1)) = -G(2*a+1); // -Gy K(numDOFs(2*a+1), numDOFs(2*b+1)) = L(a,b); // bubble } } //opserr<<"K = "<<K; return K; }
// Pass 1, to be called from tip to base. template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::multiplyByMInvPass1Inward( const SBInstanceCache& ic, const SBTreePositionCache& pc, const SBArticulatedBodyInertiaCache& abc, const Real* jointForces, SpatialVec* allZ, SpatialVec* allZPlus, Real* allEpsilon) const { const Vec<dof>& f = fromU(jointForces); SpatialVec& z = allZ[nodeNum]; SpatialVec& zPlus = allZPlus[nodeNum]; Vec<dof>& eps = toU(allEpsilon); const bool isPrescribed = isUDotKnown(ic); const HType& H = getH(pc); const HType& G = getG(abc); z = 0; for (unsigned i=0; i<children.size(); i++) { const PhiMatrix& phiChild = children[i]->getPhi(pc); const SpatialVec& zPlusChild = allZPlus[children[i]->getNodeNum()]; z += phiChild * zPlusChild; // 18 flops } zPlus = z; if (!isPrescribed) { eps = f - ~H*z; // 12*dof flops zPlus += G*eps; // 12*dof flops } }
int getG(long long x, int low = 2) { if (x <= maxn && g[x] != -1) { return g[x]; } printf("%lld\n", x); long long ans = -1; int sqr = sqrt(x + 0.5); for (int i = low; i <= sqr; ++i) { if (x % i == 0) { if (x / i % i == 0) { ans = getMu(x / i, i) * powmod(i, K); } else { ans = getMu(x / i, i) * powmod(i, K) - getG(x / i, i); } break; } } if (ans == -1) { ans = powmod(x, K) - 1; } if (x <= maxn) { g[x] = ans; } return ans; }
// Pass 2 of multiplyByMInv. // Base to tip: temp allA_GB does not need to be initialized before // beginning the iteration. template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::multiplyByMInvPass2Outward( const SBInstanceCache& ic, const SBTreePositionCache& pc, const SBArticulatedBodyInertiaCache& abc, const Real* allEpsilon, SpatialVec* allA_GB, Real* allUDot) const { const Vec<dof>& eps = fromU(allEpsilon); SpatialVec& A_GB = allA_GB[nodeNum]; Vec<dof>& udot = toU(allUDot); // pull out this node's udot const bool isPrescribed = isUDotKnown(ic); const HType& H = getH(pc); const PhiMatrix& phi = getPhi(pc); const Mat<dof,dof>& DI = getDI(abc); const HType& G = getG(abc); // Shift parent's acceleration outward (Ground==0). 12 flops const SpatialVec& A_GP = allA_GB[parent->getNodeNum()]; const SpatialVec APlus = ~phi * A_GP; // For a prescribed mobilizer, set udot==0. if (isPrescribed) { udot = 0; A_GB = APlus; } else { udot = DI*eps - ~G*APlus; // 2dof^2 + 11 dof flops A_GB = APlus + H*udot; // 12 dof flops } }
JNIEXPORT void JNICALL Java_jp_dego_sample_ipcv_MainActivity_getGrayScale(JNIEnv *env, jobject obj, jintArray pix, jint w, jint h) { unsigned char r, g, b, gray; jint* pixels = (*env)->GetIntArrayElements(env, pix, 0); int i; for (i = 0; i < w * h; i++) { // rgb 抽出 // r = (pixels[i] & 0x00FF0000) >> 16; // g = (pixels[i] & 0x0000FF00) >> 8; // b = (pixels[i] & 0x000000FF); r = getR(pixels[i]); g = getG(pixels[i]); b = getB(pixels[i]); // グレイスケールの輝度値を計算 gray = (r + g + b) / 3; if (gray > 255) gray = 255; // 色データ組み換え pixels[i] = 0xFF000000 | (gray << 16) | (gray << 8) | gray; // pixels[i] = gray; } // 確保したメモリを開放 (*env)->ReleaseIntArrayElements(env, pix, pixels, 0); }
bool WindowBitmap::rgbEqual(int x1, int y1, BYTE r, BYTE g, BYTE b) { int r1 = getR(x1, y1); int g1 = getG(x1, y1); int b1 = getB(x1, y1); return (r1 == r) && (g1 == g) && (b1 == b); }
bool WindowBitmap::rgbLarger(int x1, int y1, BYTE r, BYTE g, BYTE b) { int r1 = getR(x1, y1); int g1 = getG(x1, y1); int b1 = getB(x1, y1); return (r1 > r) && (g1 > g) && (b1 > b); }
bool WindowBitmap::rgbLess(int x1, int y1, BYTE r, BYTE g, BYTE b) { int r1 = getR(x1, y1); int g1 = getG(x1, y1); int b1 = getB(x1, y1); return (r1 < r) && (g1 < g) && (b1 < b); }
bool WindowBitmap::rgbNear(int x1, int y1, BYTE r, BYTE g, BYTE b) { int r1 = getR(x1, y1); int g1 = getG(x1, y1); int b1 = getB(x1, y1); return (abs(r1-r)<=10) && (abs(g1-g)<=10) && (abs(b1-b)<=10); }
/* int Dstar::computeShortestPath() * -------------------------- * As per [S. Koenig, 2002] except for 2 main modifications: * 1. We stop planning after a number of steps, 'maxsteps' we do this * because this algorithm can plan forever if the start is * surrounded by obstacles. * 2. We lazily remove states from the open list so we never have to * iterate through it. */ int Dstar::computeShortestPath() { list<state> s; list<state>::iterator i; if (openList.empty()) return 1; int k=0; while ((!openList.empty()) && (openList.top() < (calculateKey(s_start))) || (!isConsistent(s_start))) { if (k++ > maxSteps) { fprintf(stderr, "At maxsteps\n"); return -1; } state u; // check consistency (one of the loop conditions) bool test = isConsistent(s_start); //(getRHS(s_start) != getG(s_start)); // lazy remove while(1) { if (openList.empty()) return 1; // checks outer loop condition #1 u = openList.top(); if (!queuePop()) continue; if (!(u < s_start) && test) return 2; // checks outer loop conditions #2,3 still hold break; } state k_old = u; if (k_old < calculateKey(u)) { // u is out of date insert(u); // u has been removed already, reinsert into pq with new key } else if (getG(u) > getRHS(u)) { // needs update (got better) setG(u,getRHS(u)); getPred(u,s); for (i=s.begin();i != s.end(); i++) { updateVertex(*i); } } else { // g <= rhs, state has got worse setG(u,INFINITY); getPred(u,s); for (i=s.begin();i != s.end(); i++) { updateVertex(*i); } updateVertex(u); } } return 0; }
/* state Dstar::calculateKey(state u) * -------------------------- * As per [S. Koenig, 2002] */ state Dstar::calculateKey(state u) { double val = fmin(getRHS(u),getG(u)); u.k.first = val + heuristic(u,s_start) + k_m; u.k.second = val; return u; }
bool Astar::checkOpen(int col, int row, int id) { // 检查open列表中是否有更小的步长,并排序 for(int i = _open->count() - 1; i > 0; i--) { auto item = (AstarItem*)_open->getObjectAtIndex(i); if(item->getCol() == col && item->getRow() == row) { int tempG = getG(col, row, id); if(tempG < item->getG()) { item->setG(tempG); item->setFid(id); item->setF(tempG + item->getH()); resetSort(i); } return false; } } return true; }
/* void Dstar::updateVertex(state u) * -------------------------- * As per [S. Koenig, 2002] */ void Dstar::updateVertex(state u) { list<state> s; list<state>::iterator i; if (u != s_goal) { getSucc(u,s); double tmp = INFINITY; double tmp2; for (i=s.begin();i != s.end(); i++) { tmp2 = getG(*i) + cost(u,*i); if (tmp2 < tmp) tmp = tmp2; } if (!close(getRHS(u),tmp)) setRHS(u,tmp); } if (!close(getG(u),getRHS(u))) insert(u); }
peano::applications::faxen::records::RegularGridCell peano::applications::faxen::records::RegularGridCellPacked::convert() const{ return RegularGridCell( getP(), getU(), getV(), getF(), getG(), getRhs(), getRes(), getIsInside() ); }
void Texture::lock(std::string filename, std::string maskFile, long maskColor, bool mipmap, DEXGL_TEXGEN funcTexGen) { if (filename != "") { int width, height; long *data = FileBmp::loadBmp(filename, width, height); for (int i = 1; i < width; i *= 2); for (int j = 1; j < height; j *= 2); int newWidth = i; int newHeight = j; long *newData = FileBmp::resizeBmp( data, width, height, newWidth, newHeight ); if (newData) delete []data; else newData = data; if (maskFile == "") for (long k = 0; k < (long) newWidth * newHeight; k++) newData[k] = newData[k] | 0xFF000000; else { long *mask = FileBmp::loadBmp(maskFile, width, height); long *newMask = FileBmp::resizeBmp( mask, width, height, newWidth, newHeight ); if (newMask) delete []mask; else newMask = mask; float r = (float) 1 / (255 * 3); for (long k = 0; k < (long) newWidth * newHeight; k++) if (maskColor == -1) { float alpha = (float) ( getR(newData[k]) + getG(newData[k]) + getB(newData[k]) ) * r; newData[k] = newData[k] | (((long) (alpha * 255)) << 24); } else newData[k] = (newMask[k] == maskColor) ? (newData[k] | 0xFF000000) : newData[k]; delete []newMask; } mNumID = funcTexGen(newData, newWidth, newHeight, mipmap); delete []newData; mGlow = (maskColor < 0); } }
int Astar::getG(int col, int row, int id) { auto item = (AstarItem*)_close->getObjectAtIndex(id); int fx = item->getCol(); int fy = item->getRow(); int fg = item->getG(); if(fx - col != 0 && fy - row != 0) { return fg + 14; } else { return fg + 10; } }
void Astar::addtoopen(int col,int row,int id) { //向open列表中加入点 AstarItem * temp = new AstarItem(); temp->setpos(col,row); temp->setfid(id); int g = getG(col, row, id); int h = getH(col, row); temp->setg(g); temp->seth(h); temp->setf(g + h); open->addObject(temp); resetSort(open->count() - 1); }
//---------------------------------------------------------------------------------- void FunctionsSingleton::coutAll() const { std::cout << getD() << std::endl; std::cout << getF() << std::endl; std::cout << getG() << std::endl; std::cout << getM() << std::endl; std::cout << getN() << std::endl; std::cout << getP() << std::endl; std::cout << getQ() << std::endl; std::cout << getR() << std::endl; std::cout << getW() << std::endl; std::cout << "Alpha: " << mAlpha << std::endl; std::cout << "Beta: " << mBeta << std::endl; std::cout << "Gamma: " << mGamma << std::endl; }
void Astar::addToOpen(int col, int row, int id) { CC_ASSERT(_open != nullptr && "the open is nullptr"); AstarItem* temp = AstarItem::create(); temp->setPos(col, row); temp->setFid(id); int g = getG(col, row, id); int h = getH(col, row); temp->setG(g); temp->setH(h); temp->setF(g +h); _open->addObject(temp); resetSort(_open->count() - 1); }
//-------------------------------------------------------- void Pixelgroup::paint(PixelWriterInterface& writer) { // safeties if (!isDirty() && !m_isFlickering) return; if (getPixels() == 0 || getSize() == 0) return; bool strobeState = !Strobe::isStrobing() || Strobe::getStrobeState(); bool flickeringDone = false; // mark as clean setDirty(false); for (uint8_t i=0; i<getSize(); i++) { uint8_t pixelIndex = getPixels()[i]; if (m_isFlickering > 0 && !flickeringDone) { #ifdef ESP8266 if ((rand() % 100) > 80) { #else if ((random() % 100) > 80) { #endif strobeState &= 0; flickeringDone = true; m_isFlickering--; // mark as dirty to redraw on next cycle setDirty(true); } } // set pixel color (or black) // sets color in pixelWriter // sets pixelwriter to dirty if (!strobeState || flickeringDone) { writer.setPixelColor(pixelIndex, 0, 0, 0); } else { writer.setPixelColor((uint8_t)pixelIndex, getR(), getG(), getB()); } } }
int main() { calcprime(); calcmu(); memset(g, -1, sizeof g); g[1] = 1; long long N; scanf("%lld %d", &N, &K); long long ans = 0; for (int i = 1; i <= N; ++i) { ans += N / i * N / i % mod * getG(i) % mod; ans %= mod; } printf("%lld\n", ans); return 0; }
//============================================================================== // CALC UDOT //============================================================================== // To be called from tip to base. // Temps do not need to be initialized. // // First pass // ---------- // Given previously-calculated quantities from the State // P this body's articulated body inertia // Phi composite body child-to-parent shift matrix // H joint transition matrix; sense is transposed from Jain // G P * H * DI // and supplied arguments // F body force applied to B // f generalize forces applied to B's mobilities // udot_p known udots (if mobilizer is prescribed) // calculate // z articulated body residual force on B // zPlus AB residual force as felt on inboard side of mobilizer // eps f - ~H*z gen force plus gen equiv of body forces // For a prescribed mobilizer we have zPlus=z. Otherwise, zPlus = z + G*eps. // // This is the first (inward) loop of Algorithm 16.2 on page 323 of Jain's // 2011 book, modified to include the applied body force and not including // the auxiliary quantity "nu=DI*eps". template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcUDotPass1Inward( const SBInstanceCache& ic, const SBTreePositionCache& pc, const SBArticulatedBodyInertiaCache& abc, const SBDynamicsCache& dc, const Real* jointForces, const SpatialVec* bodyForces, const Real* allUDot, SpatialVec* allZ, SpatialVec* allZPlus, Real* allEpsilon) const { const Vec<dof>& f = fromU(jointForces); const SpatialVec& F = bodyForces[nodeNum]; SpatialVec& z = allZ[nodeNum]; SpatialVec& zPlus = allZPlus[nodeNum]; Vec<dof>& eps = toU(allEpsilon); const bool isPrescribed = isUDotKnown(ic); const HType& H = getH(pc); const ArticulatedInertia& P = getP(abc); const HType& G = getG(abc); // z = Pa+b - F z = getMobilizerCentrifugalForces(dc) - F; // 6 flops // z += P H udot_p if prescribed if (isPrescribed && !isUDotKnownToBeZero(ic)) { const Vec<dof>& udot = fromU(allUDot); z += P*(H*udot); // 66+12*dof flops } // z += sum(Phi(child) * zPlus(child)) for all children for (unsigned i=0; i<children.size(); ++i) { const PhiMatrix& phiChild = children[i]->getPhi(pc); const SpatialVec& zPlusChild = allZPlus[children[i]->getNodeNum()]; z += phiChild * zPlusChild; // 18 flops } eps = f - ~H*z; // 12*dof flops zPlus = z; if (!isPrescribed) zPlus += G*eps; // 12*dof flops }
bool Astar::checkOpen(int col,int row,int id) { //检查open列表中是否有更小的步长,并排序 for(int i = open->count() - 1;i > 0;i --){ if(((AstarItem *)open->objectAtIndex(i))->getcol() == col && ((AstarItem *)open->objectAtIndex(i))->getrow() == row){ int tempG = getG(col,row,id); if(tempG < ((AstarItem *)open->objectAtIndex(i))->getg()){ ((AstarItem *)open->objectAtIndex(i))->setg(tempG); ((AstarItem *)open->objectAtIndex(i))->setfid(id); ((AstarItem *)open->objectAtIndex(i))->setf(tempG + ((AstarItem *)open->objectAtIndex(i))->geth()); resetSort(i); } return false; } } return true; }
void peano::applications::faxen::records::RegularGridCellPacked::toString (std::ostream& out) const { out << "("; out << "P:" << getP(); out << ","; out << "U:" << getU(); out << ","; out << "V:" << getV(); out << ","; out << "F:" << getF(); out << ","; out << "G:" << getG(); out << ","; out << "rhs:" << getRhs(); out << ","; out << "res:" << getRes(); out << ","; out << "isInside:" << getIsInside(); out << ")"; }
// // Calculate acceleration in internal coordinates, based on the last set // of forces that were reduced into epsilon (e.g., see above). // Base to tip: temp allA_GB does not need to be initialized before // beginning the iteration. // template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcUDotPass2Outward( const SBInstanceCache& ic, const SBTreePositionCache& pc, const SBArticulatedBodyInertiaCache& abc, const SBTreeVelocityCache& vc, const SBDynamicsCache& dc, const Real* allEpsilon, SpatialVec* allA_GB, Real* allUDot, Real* allTau) const { const Vec<dof>& eps = fromU(allEpsilon); SpatialVec& A_GB = allA_GB[nodeNum]; Vec<dof>& udot = toU(allUDot); // pull out this node's udot const bool isPrescribed = isUDotKnown(ic); const HType& H = getH(pc); const PhiMatrix& phi = getPhi(pc); const ArticulatedInertia& P = getP(abc); const SpatialVec& a = getMobilizerCoriolisAcceleration(vc); const Mat<dof,dof>& DI = getDI(abc); const HType& G = getG(abc); // Shift parent's acceleration outward (Ground==0). 12 flops const SpatialVec& A_GP = allA_GB[parent->getNodeNum()]; const SpatialVec APlus = ~phi * A_GP; if (isPrescribed) { Vec<dof>& tau = updTau(ic,allTau); // pull out this node's tau // This is f - ~H(P*APlus + z); compare Jain 16.17b. Note sign // change since our tau is on the LHS while his is on the RHS. tau = eps - ~H*(P*APlus); // 66 + 12*dof flops } else { udot = DI*eps - ~G*APlus; // 2*dof^2 + 11*dof } A_GB = APlus + H*udot + a; }