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;
}
