XC::MidDistanceBeamIntegration::MidDistanceBeamIntegration(int nIP,const Vector &pt)
: ParameterIDBeamIntegration(BEAM_INTEGRATION_TAG_MidDistance,pt)
{
for(int i = 0; i < nIP; i++)
{
int key = i;
for(int j = i+1; j < nIP; j++)
{
if(pts(j) < pts(key))
{
key = j;
std::cerr << "MidDistanceBeamIntegration::MidDistanceBeamIntegration -- point are not sorted; sort before calling constructor" << std::endl;
}
}
//double temp = pts(i);
//pts(i) = pts(key);
//pts(key) = temp;
}
Vector mids(nIP-1);
for(int i = 0; i < nIP-1; i++)
{
mids(i) = 0.5*(pts(i)+pts(i+1));
}
wts(0) = mids(0);
wts(nIP-1) = 1.0-mids(nIP-2);
for(int i = 1; i < nIP-1; i++)
{ wts(i) = mids(i)-mids(i-1); }
}
ResPolyRing* readRing(std::istream& infile)
{
// Hack job.
// Input format:
// <characteristic>
// <n:#variables>
// <weight_0> <weight_1> ... <weight_(n-1)>
// grevlex | lex | ...
int charac;
int nvars;
infile >> charac;
infile >> nvars;
std::vector<int> wts(nvars);
for (int i=0; i<nvars; i++)
{
int a;
infile >> a;
wts.push_back(a);
}
MonomialOrdering mo(nvars, wts);
MonomialInfo M(mo);
M2::ARingZZp kk(101);
ResGausser G(kk);
ResPolyRing* R = new ResPolyRing(G,M);
return R;
}
int main(int argc, char *argv[])
{
std::vector<double> min_p = {-1000, -2000};
std::vector<double> max_p = {1000, 4000};
std::vector<double> wts(2000, 0.6); // each of 2000 iterations have weight 0.6
// perform ppso
auto ret = ppso::Particle_Swarm{
objective_function(),
2, // number of dimensions
min_p, // vector of minimum values for each dimension
max_p, // vector of maximum values for each dimension
std::max(std::thread::hardware_concurrency(), (unsigned int)2), // number of groups (threads)
20, // number of particles
2000.0, // maximum velocity
wts, // vector of weights for each iteration
2, // personal influence constant
2, // group influence constant
4, // every 4 iterations, communication strategy 1
8, // every 8 iterations, communication strategy 2
2000 // 2000 iterations
}.run();
// print x*
std::cout << std::fixed << "x* = [ ";
for (double x: ret.first) {
std::cout << x << " ";
}
std::cout << "]'\n";
// print f(x*)
std::cout << "f(x*) = " << ret.second << std::endl;
return 0;
}
LowOrderBeamIntegration::LowOrderBeamIntegration(int nIP,
const Vector &pt,
int nc,
const Vector &wc):
BeamIntegration(BEAM_INTEGRATION_TAG_LowOrder),
pts(nIP), wts(nIP), Nc(nc), parameterID(0)
{
for (int i = 0; i < nIP; i++) {
if (pt(i) < 0.0 || pt(i) > 1.0)
opserr << "LowOrderBeamIntegration::LowOrderBeamIntegration -- point lies outside [0,1]" << endln;
pts(i) = pt(i);
}
int nf = nIP-nc;
if (nf > 0) {
Vector R(nf);
for (int i = 0; i < nf; i++) {
double sum = 0.0;
for (int j = 0; j < nc; j++)
sum += pow(pts(j),i)*wc(j);
R(i) = 1.0/(i+1) - sum;
}
Matrix J(nf,nf);
for (int i = 0; i < nf; i++)
for (int j = 0; j < nf; j++)
J(i,j) = pow(pts(nc+j),i);
Vector wf(nf);
J.Solve(R, wf);
for (int i = 0; i < nf; i++)
wts(nc+i) = wf(i);
for (int i = 0; i < nc; i++)
wts(i) = wc(i);
}
else
wts = wc;
}
void XC::MidDistanceBeamIntegration::getSectionWeights(int numSections, double L, double *wt)
{
const int nIP = wts.Size();
int i;
for(i = 0; i < nIP; i++)
wt[i] = wts(i);
for( ; i < numSections; i++)
wt[i] = 1.0;
}
void
FixedLocationBeamIntegration::getSectionWeights(int numSections,
double L, double *wt)
{
int nIP = wts.Size();
int i;
for (i = 0; i < nIP; i++)
wt[i] = wts(i);
for ( ; i < numSections; i++)
wt[i] = 1.0;
}
SingletonKusuoka( const Teuchos::RCP<Distribution<Real> > &dist,
const int nQuad,
const Teuchos::RCP<PlusFunction<Real> > &pf)
: RiskMeasure<Real>() {
// Build generalized trapezoidal rule
std::vector<Real> wts(nQuad), pts(nQuad);
buildQuadFromDist(pts,wts,nQuad,dist);
// Build mixed quantile quadrangle risk measure
buildMixedQuantile(pts,wts,pf);
// Check inputs
checkInputs(dist);
}
void
LowOrderBeamIntegration::Print(OPS_Stream &s, int flag)
{
s << "LowOrder" << endln;
s << " Points: " << pts;
s << " Weights: " << wts;
double sum = 0.0;
int N = wts.Size();
for (int i = 0; i < N; i++)
sum += fabs(wts(i));
s << " Condition Number: " << sum << endln;
}
void
MidDistanceBeamIntegration::Print(OPS_Stream &s, int flag)
{
if (flag == OPS_PRINT_PRINTMODEL_JSON) {
s << "{\"type\": \"MidDistance\", ";
s << "\"points\": [";
int nIP = pts.Size();
for (int i = 0; i < nIP-1; i++)
s << pts(i) << ", ";
s << pts(nIP-1) << "], ";
s << "\"weights\": [";
nIP = wts.Size();
for (int i = 0; i < nIP-1; i++)
s << wts(i) << ", ";
s << wts(nIP-1) << "]}";
}
else {
s << "MidDistance" << endln;
s << " Points: " << pts;
s << " Weights: " << wts;
}
}
RDNumeric::DoubleVector *_translateWeights(python::object weights) {
PySequenceHolder<double> wts(weights);
unsigned int nwts = wts.size();
RDNumeric::DoubleVector *wtsVec;
wtsVec = 0;
unsigned int i;
if (nwts > 0) {
wtsVec = new RDNumeric::DoubleVector(nwts);
for (i = 0; i < nwts; i++) {
wtsVec->setVal(i, wts[i]);
}
}
return wtsVec;
}
MidDistanceBeamIntegration::MidDistanceBeamIntegration(int nIP,
const Vector &pt):
BeamIntegration(BEAM_INTEGRATION_TAG_MidDistance),
pts(nIP), wts(nIP)
{
for (int i = 0; i < nIP; i++) {
if (pt(i) < 0.0 || pt(i) > 1.0)
opserr << "MidDistanceBeamIntegration::MidDistanceBeamIntegration -- point lies outside [0,1]" << endln;
pts(i) = pt(i);
}
for (int i = 0; i < nIP; i++) {
int key = i;
for (int j = i+1; j < nIP; j++) {
if (pts(j) < pts(key)) {
key = j;
opserr << "MidDistanceBeamIntegration::MidDistanceBeamIntegration -- point are not sorted; sort before calling constructor" << endln;
}
}
//double temp = pts(i);
//pts(i) = pts(key);
//pts(key) = temp;
}
Vector mids(nIP-1);
for (int i = 0; i < nIP-1; i++) {
mids(i) = 0.5*(pts(i)+pts(i+1));
}
wts(0) = mids(0);
wts(nIP-1) = 1.0-mids(nIP-2);
for (int i = 1; i < nIP-1; i++) {
wts(i) = mids(i)-mids(i-1);
}
}
void
LowOrderBeamIntegration::getSectionWeights(int numSections,
double L, double *wt)
{
int nIP = wts.Size();
int Nf = nIP-Nc;
if (Nf > 0) {
Vector R(Nf);
for (int i = 0; i < Nf; i++) {
double sum = 0.0;
for (int j = 0; j < Nc; j++)
sum += pow(pts(j),i)*wts(j);
R(i) = 1.0/(i+1) - sum;
}
Matrix J(Nf,Nf);
for (int i = 0; i < Nf; i++)
for (int j = 0; j < Nf; j++)
J(i,j) = pow(pts(Nc+j),i);
Vector wf(Nf);
J.Solve(R, wf);
for (int i = 0; i < Nf; i++)
wts(Nc+i) = wf(i);
}
int i;
for (i = 0; i < nIP; i++)
wt[i] = wts(i);
for ( ; i < numSections; i++)
wt[i] = 1.0;
}
ProcessStarterPrivate::_phandle ProcessStarterPrivate::GetCurrentUserToken()
{
QLibrary wts("Wtsapi32");
BOOL (__stdcall * pWTSQueryUserToken)(ULONG SessionId,PHANDLE phToken) =
(BOOL(__stdcall *)(ULONG,PHANDLE)) wts.resolve("WTSQueryUserToken");
BOOL (__stdcall * pWTSEnumerateSessionsW)(HANDLE hServer,DWORD Reserved,DWORD Version,
PWTS_SESSION_INFOW * ppSessionInfo,DWORD * pCount) =
(BOOL(__stdcall *)(HANDLE, DWORD,DWORD,PWTS_SESSION_INFOW *, DWORD *))
wts.resolve("WTSEnumerateSessionsW");
PHANDLE currentToken = &curTok;
PHANDLE primaryToken = &primTok;
int dwSessionId = 0;
PWTS_SESSION_INFOW pSessionInfo = 0;
DWORD dwCount = 0;
pWTSEnumerateSessionsW(WTS_CURRENT_SERVER_HANDLE, 0, 1, &pSessionInfo, &dwCount);
for (DWORD i = 0; i < dwCount; ++i)
{
WTS_SESSION_INFO si = pSessionInfo[i];
if (WTSActive == si.State)
{
dwSessionId = si.SessionId;
break;
}
}
BOOL bRet = pWTSQueryUserToken(dwSessionId, currentToken);
int errorcode = GetLastError();
if (bRet == false)
{
return 0;
}
bRet = DuplicateTokenEx(*currentToken, TOKEN_ASSIGN_PRIMARY | TOKEN_ALL_ACCESS, 0, SecurityImpersonation, TokenPrimary, primaryToken);
errorcode = GetLastError();
if (bRet == false)
{
return 0;
}
return primaryToken;
}
int main()
{
int nt = 4;
std::vector<double> nds(nt);
std::vector<double> wts(nt);
cdgqf ( nt, 1, 1., 1., &nds[0], &wts[0] );
for( int k = 0; k < nt; k++ )
{
std::cout << "nd = " << std::setprecision(16) << nds[k] << "\t"
<< "wt = " << std::setprecision(16) << wts[k] << std::endl;
}
return 0;
}
SingletonKusuoka(Teuchos::ParameterList &parlist)
: RiskMeasure<Real>() {
// Parse parameter list
Teuchos::ParameterList &list
= parlist.sublist("SOL").sublist("Risk Measure").sublist("Singleton Kusuoka");
int nQuad = list.get("Number of Quadrature Points",5);
// Build distribution
Teuchos::RCP<Distribution<Real> > dist = DistributionFactory<Real>(list);
// Build plus function approximation
Teuchos::RCP<PlusFunction<Real> > pf = Teuchos::rcp(new PlusFunction<Real>(list));
// Build generalized trapezoidal rule
std::vector<Real> wts(nQuad), pts(nQuad);
buildQuadFromDist(pts,wts,nQuad,dist);
// Build mixed quantile quadrangle risk measure
buildMixedQuantile(pts,wts,pf);
// Check inputs
checkInputs(dist);
}
void Foam::FitData<FitDataType, ExtendedStencil, Polynomial>::calcFit
(
scalarList& coeffsi,
const List<point>& C,
const scalar wLin,
const label facei
)
{
vector idir(1,0,0);
vector jdir(0,1,0);
vector kdir(0,0,1);
findFaceDirs(idir, jdir, kdir, facei);
// Setup the point weights
scalarList wts(C.size(), scalar(1));
wts[0] = centralWeight_;
if (linearCorrection_)
{
wts[1] = centralWeight_;
}
// Reference point
point p0 = this->mesh().faceCentres()[facei];
// Info << "Face " << facei << " at " << p0 << " stencil points at:\n"
// << C - p0 << endl;
// p0 -> p vector in the face-local coordinate system
vector d;
// Local coordinate scaling
scalar scale = 1;
// Matrix of the polynomial components
scalarRectangularMatrix B(C.size(), minSize_, scalar(0));
for(label ip = 0; ip < C.size(); ip++)
{
const point& p = C[ip];
d.x() = (p - p0)&idir;
d.y() = (p - p0)&jdir;
# ifndef SPHERICAL_GEOMETRY
d.z() = (p - p0)&kdir;
# else
d.z() = mag(p) - mag(p0);
# endif
if (ip == 0)
{
scale = cmptMax(cmptMag((d)));
}
// Scale the radius vector
d /= scale;
Polynomial::addCoeffs
(
B[ip],
d,
wts[ip],
dim_
);
}
// Additional weighting for constant and linear terms
for(label i = 0; i < B.n(); i++)
{
B[i][0] *= wts[0];
B[i][1] *= wts[0];
}
// Set the fit
label stencilSize = C.size();
coeffsi.setSize(stencilSize);
bool goodFit = false;
for(int iIt = 0; iIt < 8 && !goodFit; iIt++)
{
SVD svd(B, SMALL);
scalar maxCoeff = 0;
label maxCoeffi = 0;
for(label i=0; i<stencilSize; i++)
{
coeffsi[i] = wts[0]*wts[i]*svd.VSinvUt()[0][i];
if (mag(coeffsi[i]) > maxCoeff)
{
maxCoeff = mag(coeffsi[i]);
maxCoeffi = i;
}
}
if (linearCorrection_)
{
goodFit =
(mag(coeffsi[0] - wLin) < linearLimitFactor_*wLin)
&& (mag(coeffsi[1] - (1 - wLin)) < linearLimitFactor_*(1 - wLin))
&& maxCoeffi <= 1;
}
else
{
// Upwind: weight on face is 1.
goodFit =
(mag(coeffsi[0] - 1.0) < linearLimitFactor_*1.0)
&& maxCoeffi <= 1;
}
// if (goodFit && iIt > 0)
// {
// Info << "FitData<Polynomial>::calcFit"
// << "(const List<point>& C, const label facei" << nl
// << "Can now fit face " << facei << " iteration " << iIt
// << " with sum of weights " << sum(coeffsi) << nl
// << " Weights " << coeffsi << nl
// << " Linear weights " << wLin << " " << 1 - wLin << nl
// << " sing vals " << svd.S() << endl;
// }
if (!goodFit) // (not good fit so increase weight in the centre and weight
// for constant and linear terms)
{
// if (iIt == 7)
// {
// WarningIn
// (
// "FitData<Polynomial>::calcFit"
// "(const List<point>& C, const label facei"
// ) << "Cannot fit face " << facei << " iteration " << iIt
// << " with sum of weights " << sum(coeffsi) << nl
// << " Weights " << coeffsi << nl
// << " Linear weights " << wLin << " " << 1 - wLin << nl
// << " sing vals " << svd.S() << endl;
// }
wts[0] *= 10;
if (linearCorrection_)
{
wts[1] *= 10;
}
for(label j = 0; j < B.m(); j++)
{
B[0][j] *= 10;
B[1][j] *= 10;
}
for(label i = 0; i < B.n(); i++)
{
B[i][0] *= 10;
B[i][1] *= 10;
}
}
}
if (goodFit)
{
if (linearCorrection_)
{
// Remove the uncorrected linear coefficients
coeffsi[0] -= wLin;
coeffsi[1] -= 1 - wLin;
}
else
{
// Remove the uncorrected upwind coefficients
coeffsi[0] -= 1.0;
}
}
else
{
// if (debug)
// {
WarningIn
(
"FitData<Polynomial>::calcFit(..)"
) << "Could not fit face " << facei
<< " Weights = " << coeffsi
<< ", reverting to linear." << nl
<< " Linear weights " << wLin << " " << 1 - wLin << endl;
// }
coeffsi = 0;
}
}
void Foam::UpwindFitData<Polynomial>::calcFit()
{
const fvMesh& mesh = this->mesh();
const surfaceScalarField& w = mesh.surfaceInterpolation::weights();
const surfaceScalarField::GeometricBoundaryField& bw = w.boundaryField();
// Owner stencil weights
// ~~~~~~~~~~~~~~~~~~~~~
// Get the cell/face centres in stencil order.
List<List<point> > stencilPoints(mesh.nFaces());
this->stencil().collectData
(
this->stencil().ownMap(),
this->stencil().ownStencil(),
mesh.C(),
stencilPoints
);
// find the fit coefficients for every owner
//Pout<< "-- Owner --" << endl;
for (label facei = 0; facei < mesh.nInternalFaces(); facei++)
{
scalarList wts(stencilPoints[facei].size(), scalar(1));
FitData
<
UpwindFitData<Polynomial>,
extendedUpwindCellToFaceStencil,
Polynomial
>::calcFit(owncoeffs_[facei], wts, stencilPoints[facei], w[facei], facei);
//Pout<< " facei:" << facei
// << " at:" << mesh.faceCentres()[facei] << endl;
//forAll(owncoeffs_[facei], i)
//{
// Pout<< " point:" << stencilPoints[facei][i]
// << "\tweight:" << owncoeffs_[facei][i]
// << endl;
//}
}
forAll(bw, patchi)
{
const fvsPatchScalarField& pw = bw[patchi];
if (pw.coupled())
{
label facei = pw.patch().start();
forAll(pw, i)
{
scalarList wts(stencilPoints[facei].size(), scalar(1));
FitData
<
UpwindFitData<Polynomial>,
extendedUpwindCellToFaceStencil,
Polynomial
>::calcFit
(
owncoeffs_[facei], wts, stencilPoints[facei], pw[i], facei
);
facei++;
}
}
Foam::label Foam::quadraticFitSnGradData::calcFit
(
const List<point>& C,
const label faci
)
{
vector idir(1,0,0);
vector jdir(0,1,0);
vector kdir(0,0,1);
findFaceDirs(idir, jdir, kdir, mesh(), faci);
scalarList wts(C.size(), scalar(1));
wts[0] = centralWeight_;
wts[1] = centralWeight_;
point p0 = mesh().faceCentres()[faci];
scalar scale = 0;
// calculate the matrix of the polynomial components
scalarRectangularMatrix B(C.size(), minSize_, scalar(0));
for(label ip = 0; ip < C.size(); ip++)
{
const point& p = C[ip];
scalar px = (p - p0)&idir;
scalar py = (p - p0)&jdir;
#ifdef SPHERICAL_GEOMETRY
scalar pz = mag(p) - mag(p0);
#else
scalar pz = (p - p0)&kdir;
#endif
if (ip == 0) scale = max(max(mag(px), mag(py)), mag(pz));
px /= scale;
py /= scale;
pz /= scale;
label is = 0;
B[ip][is++] = wts[0]*wts[ip];
B[ip][is++] = wts[0]*wts[ip]*px;
B[ip][is++] = wts[ip]*sqr(px);
if (dim_ >= 2)
{
B[ip][is++] = wts[ip]*py;
B[ip][is++] = wts[ip]*px*py;
B[ip][is++] = wts[ip]*sqr(py);
}
if (dim_ == 3)
{
B[ip][is++] = wts[ip]*pz;
B[ip][is++] = wts[ip]*px*pz;
//B[ip][is++] = wts[ip]*py*pz;
B[ip][is++] = wts[ip]*sqr(pz);
}
}
// Set the fit
label stencilSize = C.size();
fit_[faci].setSize(stencilSize);
scalarList singVals(minSize_);
label nSVDzeros = 0;
const scalar& deltaCoeff = mesh().deltaCoeffs()[faci];
bool goodFit = false;
for(int iIt = 0; iIt < 10 && !goodFit; iIt++)
{
SVD svd(B, SMALL);
scalar fit0 = wts[0]*wts[0]*svd.VSinvUt()[1][0]/scale;
scalar fit1 = wts[0]*wts[1]*svd.VSinvUt()[1][1]/scale;
goodFit =
fit0 < 0 && fit1 > 0
&& mag(fit0 + deltaCoeff) < 0.5*deltaCoeff
&& mag(fit1 - deltaCoeff) < 0.5*deltaCoeff;
if (goodFit)
{
fit_[faci][0] = fit0;
fit_[faci][1] = fit1;
for(label i = 2; i < stencilSize; i++)
{
fit_[faci][i] = wts[0]*wts[i]*svd.VSinvUt()[1][i]/scale;
}
singVals = svd.S();
nSVDzeros = svd.nZeros();
}
else // (not good fit so increase weight in the centre and for linear)
{
wts[0] *= 10;
wts[1] *= 10;
for(label i = 0; i < B.n(); i++)
{
B[i][0] *= 10;
B[i][1] *= 10;
}
for(label j = 0; j < B.m(); j++)
{
B[0][j] *= 10;
B[1][j] *= 10;
}
}
}
if (goodFit)
{
// remove the uncorrected snGradScheme coefficients
fit_[faci][0] += deltaCoeff;
fit_[faci][1] -= deltaCoeff;
}
else
{
Pout<< "quadratifFitSnGradData could not fit face " << faci
<< " fit_[faci][0] = " << fit_[faci][0]
<< " fit_[faci][1] = " << fit_[faci][1]
<< " deltaCoeff = " << deltaCoeff << endl;
fit_[faci] = 0;
}
return minSize_ - nSVDzeros;
}
const XC::Vector &XC::DispBeamColumn2d::getResistingForceSensitivity(int gradNumber)
{
const size_t numSections= getNumSections();
const Matrix &pts = quadRule.getIntegrPointCoords(numSections);
const Vector &wts = quadRule.getIntegrPointWeights(numSections);
double L = theCoordTransf->getInitialLength();
double oneOverL = 1.0/L;
// Zero for integration
q.Zero();
static XC::Vector qsens(3);
qsens.Zero();
// Some extra declarations
static XC::Matrix kbmine(3,3);
kbmine.Zero();
int j, k;
double d1oLdh = 0.0;
// Check if a nodal coordinate is random
bool randomNodeCoordinate = false;
static XC::ID nodeParameterID(2);
nodeParameterID(0) = theNodes[0]->getCrdsSensitivity();
nodeParameterID(1) = theNodes[1]->getCrdsSensitivity();
if(nodeParameterID(0) != 0 || nodeParameterID(1) != 0) {
randomNodeCoordinate = true;
const XC::Vector &ndICoords = theNodes[0]->getCrds();
const XC::Vector &ndJCoords = theNodes[1]->getCrds();
double dx = ndJCoords(0) - ndICoords(0);
double dy = ndJCoords(1) - ndICoords(1);
if(nodeParameterID(0) == 1) // here x1 is random
d1oLdh = dx/(L*L*L);
if(nodeParameterID(0) == 2) // here y1 is random
d1oLdh = dy/(L*L*L);
if(nodeParameterID(1) == 1) // here x2 is random
d1oLdh = -dx/(L*L*L);
if(nodeParameterID(1) == 2) // here y2 is random
d1oLdh = -dy/(L*L*L);
}
// Loop over the integration points
for(size_t i= 0; i < numSections; i++) {
int order = theSections[i]->getOrder();
const XC::ID &code = theSections[i]->getType();
double xi6 = 6.0*pts(i,0);
double wti = wts(i);
// Get section stress resultant gradient
const XC::Vector &s = theSections[i]->getStressResultant();
const XC::Vector &sens = theSections[i]->getStressResultantSensitivity(gradNumber,true);
// Perform numerical integration on internal force gradient
//q.addMatrixTransposeVector(1.0, *B, s, wts(i));
double si;
double sensi;
for(j = 0; j < order; j++) {
si = s(j)*wti;
sensi = sens(j)*wti;
switch(code(j)) {
case SECTION_RESPONSE_P:
q(0) += si;
qsens(0) += sensi;
break;
case SECTION_RESPONSE_MZ:
q(1) += (xi6-4.0)*si;
q(2) += (xi6-2.0)*si;
qsens(1) += (xi6-4.0)*sensi;
qsens(2) += (xi6-2.0)*sensi;
break;
default:
break;
}
}
if(randomNodeCoordinate) {
// Perform numerical integration to obtain basic stiffness matrix
//kb.addMatrixTripleProduct(1.0, *B, ks, wts(i)/L);
double tmp;
const XC::Matrix &ks = theSections[i]->getSectionTangent();
Matrix ka(workArea, order, 3);
ka.Zero();
for(j = 0; j < order; j++) {
switch(code(j)) {
case SECTION_RESPONSE_P:
for(k = 0; k < order; k++) {
ka(k,0) += ks(k,j)*wti;
}
break;
case SECTION_RESPONSE_MZ:
for(k = 0; k < order; k++) {
tmp = ks(k,j)*wti;
ka(k,1) += (xi6-4.0)*tmp;
ka(k,2) += (xi6-2.0)*tmp;
}
break;
default:
break;
}
}
for(j = 0; j < order; j++) {
switch (code(j)) {
case SECTION_RESPONSE_P:
for(k = 0; k < 3; k++) {
kbmine(0,k) += ka(j,k);
}
break;
case SECTION_RESPONSE_MZ:
for(k = 0; k < 3; k++) {
tmp = ka(j,k);
kbmine(1,k) += (xi6-4.0)*tmp;
kbmine(2,k) += (xi6-2.0)*tmp;
}
break;
default:
break;
}
}
}
}
static XC::Vector dqdh(3);
const XC::Vector &dAdh_u = theCoordTransf->getBasicTrialDispShapeSensitivity();
//dqdh = (1.0/L) * (kbmine * dAdh_u);
dqdh.addMatrixVector(0.0, kbmine, dAdh_u, oneOverL);
static XC::Vector dkbdh_v(3);
const XC::Vector &A_u = theCoordTransf->getBasicTrialDisp();
//dkbdh_v = (d1oLdh) * (kbmine * A_u);
dkbdh_v.addMatrixVector(0.0, kbmine, A_u, d1oLdh);
// Transform forces
static XC::Vector dummy(3); // No distributed loads
// Term 5
P = theCoordTransf->getGlobalResistingForce(qsens,dummy);
if(randomNodeCoordinate) {
// Term 1
P += theCoordTransf->getGlobalResistingForceShapeSensitivity(q,dummy);
// Term 2
P += theCoordTransf->getGlobalResistingForce(dqdh,dummy);
// Term 4
P += theCoordTransf->getGlobalResistingForce(dkbdh_v,dummy);
}
return P;
}
const XC::Vector &XC::DispBeamColumn2d::getResistingForce(void) const
{
const size_t numSections= getNumSections();
const Matrix &pts = quadRule.getIntegrPointCoords(numSections);
const Vector &wts = quadRule.getIntegrPointWeights(numSections);
// Zero for integration
q.Zero();
// Loop over the integration points
for(size_t i= 0; i < numSections; i++) {
int order = theSections[i]->getOrder();
const XC::ID &code = theSections[i]->getType();
double xi6 = 6.0*pts(i,0);
// Get section stress resultant
const XC::Vector &s = theSections[i]->getStressResultant();
// Perform numerical integration on internal force
//q.addMatrixTransposeVector(1.0, *B, s, wts(i));
double si;
for(int j = 0; j < order; j++) {
si = s(j)*wts(i);
switch(code(j)) {
case SECTION_RESPONSE_P:
q(0) += si; break;
case SECTION_RESPONSE_MZ:
q(1) += (xi6-4.0)*si; q(2) += (xi6-2.0)*si; break;
default:
break;
}
}
}
// Add effects of element loads, q = q(v) + q0
q(0) += q0[0];
q(1) += q0[1];
q(2) += q0[2];
// Vector for reactions in basic system
Vector p0Vec= p0.getVector();
P = theCoordTransf->getGlobalResistingForce(q, p0Vec);
// Subtract other external nodal loads ... P_res = P_int - P_ext
//P.addVector(1.0, load, -1.0);
P(0) -= load(0);
P(1) -= load(1);
P(2) -= load(2);
P(3) -= load(3);
P(4) -= load(4);
P(5) -= load(5);
if(isDead())
P*=dead_srf;
return P;
}
const XC::Matrix &XC::DispBeamColumn2d::getInitialBasicStiff(void) const
{
static Matrix kb(3,3);
// Zero for integral
kb.Zero();
const size_t numSections= getNumSections();
const Matrix &pts = quadRule.getIntegrPointCoords(numSections);
const Vector &wts = quadRule.getIntegrPointWeights(numSections);
double L = theCoordTransf->getInitialLength();
double oneOverL = 1.0/L;
// Loop over the integration points
for(size_t i= 0;i<numSections;i++) {
int order = theSections[i]->getOrder();
const XC::ID &code = theSections[i]->getType();
Matrix ka(workArea, order, 3);
ka.Zero();
double xi6 = 6.0*pts(i,0);
// Get the section tangent stiffness and stress resultant
const XC::Matrix &ks = theSections[i]->getInitialTangent();
// Perform numerical integration
//kb.addMatrixTripleProduct(1.0, *B, ks, wts(i)/L);
double wti = wts(i)*oneOverL;
double tmp;
int j, k;
for(j = 0; j < order; j++) {
switch(code(j)) {
case SECTION_RESPONSE_P:
for(k = 0; k < order; k++)
ka(k,0) += ks(k,j)*wti;
break;
case SECTION_RESPONSE_MZ:
for(k = 0; k < order; k++) {
tmp = ks(k,j)*wti;
ka(k,1) += (xi6-4.0)*tmp;
ka(k,2) += (xi6-2.0)*tmp;
}
break;
default:
break;
}
}
for(j = 0; j < order; j++) {
switch (code(j)) {
case SECTION_RESPONSE_P:
for(k = 0; k < 3; k++)
kb(0,k) += ka(j,k);
break;
case SECTION_RESPONSE_MZ:
for(k = 0; k < 3; k++) {
tmp = ka(j,k);
kb(1,k) += (xi6-4.0)*tmp;
kb(2,k) += (xi6-2.0)*tmp;
}
break;
default:
break;
}
}
}
return kb;
}
// Desired cart_vel used to compute the corresponding IK joint_vel
void RTKRobotArm::getIKJointVelocity(const Eigen::VectorXd& cart_vel, Eigen::VectorXd& joint_vel) {
MathLib::Vector cerr;
if(bOrientCtrl) {
cerr.Resize(6);
} else {
cerr.Resize(3);
}
copy(cart_vel, cerr);
if(joint_vel.size() != numdof) {
joint_vel.resize(numdof);
}
mIKSolver.SetTarget(cerr);
mIKSolver.SetJacobian(mKinematicChain.GetJacobian());
// Try first with all joint weights equal
MathLib::Vector wts(numdof);
wts.One();
mSensorsGroup.ReadSensors();
MathLib::Vector curr = mSensorsGroup.GetJointPositions();
if(bUseNull) {
MathLib::Vector nul(numdof);
for(int i=0; i<numdof; ++i) {
nul[i] = null_posture(i) - curr[mapping[i]];
}
mIKSolver.SetNullTarget(nul);
}
mIKSolver.SetDofsWeights(wts);
mIKSolver.Solve();
MathLib::Vector ikout = mIKSolver.GetOutput();
for(int i=0; i<numdof; ++i) {
joint_vel[i] = ikout[mapping[i]];
}
// Check if any joint is near limit
bool redo = false;
for(int i=0; i<numdof; ++i) {
if( (curr(i)> ulim(i) - DEG2RAD(5) && joint_vel[i] > 0) ||
(curr(i) < llim(i) + DEG2RAD(5) && joint_vel[i] < 0) ) {
ROS_WARN_STREAM_THROTTLE(0.5, "Reducing DOF "<<i<<" IK weight");
redo = true;
// Set the problematic joint weights to a small value
wts(i) = 0.1;
} else {
wts(i) = 1.0;
}
}
// If any joint is in problem, resolve with new joint weights
if(redo) {
mIKSolver.SetDofsWeights(wts);
mIKSolver.Solve();
MathLib::Vector ikout = mIKSolver.GetOutput();
for(int i=0; i<numdof; ++i) {
joint_vel[i] = ikout[mapping[i]];
}
}
}
/**
* Gauss-Legendre quadature.
* computes knots and weights of a Gauss-Legendre quadrature formula.
* \param a Left endpoint of interval
* \param b Right endpoint of interval
* \param _t array of abscissa
* \param _wts array of corresponding wights
*/
void gauss_legendre( double a, double b, const dvector& _t,
const dvector& _wts )
//
// Purpose:
//
// computes knots and weights of a Gauss-Legendre quadrature formula.
//
// Discussion:
//
// The user may specify the interval (A,B).
//
// Only simple knots are produced.
//
// Use routine EIQFS to evaluate this quadrature formula.
//
// Licensing:
//
// This code is distributed under the GNU LGPL license.
//
// Modified:
//
// September 2010 by Derek Seiple
//
// Author:
//
// Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
// C++ version by John Burkardt.
//
// Reference:
//
// Sylvan Elhay, Jaroslav Kautsky,
// Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
// Interpolatory Quadrature,
// ACM Transactions on Mathematical Software,
// Volume 13, Number 4, December 1987, pages 399-415.
//
// Parameters:
//
// Input, double A, B, the interval endpoints, or
// other parameters.
//
// Output, double T[NT], the knots.
//
// Output, double WTS[NT], the weights.
//
{
dvector t=(dvector&) _t;
dvector wts=(dvector&) _wts;
if( t.indexmax()!=wts.indexmax() )
{
cerr << "Incompatible sizes in void "
"mygauss_legendre(double a, double b, const dvector& _t, const dvector& _wts)"
<< endl;
ad_exit(-1);
}
t.shift(0);
wts.shift(0);
int nt = t.indexmax() + 1;
int ub = nt-1;
int i;
int k;
int l;
double al;
double ab;
double abi;
double abj;
double be;
double p;
double shft;
double slp;
double temp;
double tmp;
double zemu;
// Compute the Gauss quadrature formula for default values of A and B.
dvector aj(0,ub);
dvector bj(0,ub);
ab = 0.0;
zemu = 2.0 / ( ab + 1.0 );
for ( i = 0; i < nt; i++ )
{
aj[i] = 0.0;
}
for ( i = 1; i <= nt; i++ )
{
abi = i + ab * ( i % 2 );
abj = 2 * i + ab;
bj[i-1] = sqrt ( abi * abi / ( abj * abj - 1.0 ) );
}
// Compute the knots and weights.
if ( zemu <= 0.0 ) // Exit if the zero-th moment is not positive.
{
cout << "\n";
cout << "Fatal error!\n";
cout << " ZEMU <= 0.\n";
exit ( 1 );
}
// Set up vectors for IMTQLX.
for ( i = 0; i < nt; i++ )
{
t[i] = aj[i];
}
wts[0] = sqrt ( zemu );
for ( i = 1; i < nt; i++ )
{
wts[i] = 0.0;
}
// Diagonalize the Jacobi matrix.
imtqlx (t, bj, wts );
for ( i = 0; i < nt; i++ )
{
wts[i] = wts[i] * wts[i];
}
// Prepare to scale the quadrature formula to other weight function with
// valid A and B.
ivector mlt(0,ub);
for ( i = 0; i < nt; i++ )
{
mlt[i] = 1;
}
ivector ndx(0,ub);
for ( i = 0; i < nt; i++ )
{
ndx[i] = i + 1;
}
dvector st(0,ub);
dvector swts(0,ub);
temp = 3.0e-14;
al = 0.0;
be = 0.0;
if ( fabs ( b - a ) <= temp )
{
cout << "\n";
cout << "Fatal error!\n";
cout << " |B - A| too small.\n";
exit ( 1 );
}
shft = ( a + b ) / 2.0;
slp = ( b - a ) / 2.0;
p = pow ( slp, al + be + 1.0 );
for ( k = 0; k < nt; k++ )
{
st[k] = shft + slp * t[k];
l = abs ( ndx[k] );
if ( l != 0 )
{
tmp = p;
for ( i = l - 1; i <= l - 1 + mlt[k] - 1; i++ )
{
swts[i] = wts[i] * tmp;
tmp = tmp * slp;
}
}
}
for(i=0;i<nt;i++)
{
t(i) = st(ub-i);
wts(i) = swts(ub-i);
}
return;
}
void Foam::CentredFitSnGradData<Polynomial>::calcFit
(
scalarList& coeffsi,
const List<point>& C,
const scalar wLin,
const scalar deltaCoeff,
const label facei
)
{
vector idir(1,0,0);
vector jdir(0,1,0);
vector kdir(0,0,1);
this->findFaceDirs(idir, jdir, kdir, facei);
// Setup the point weights
scalarList wts(C.size(), scalar(1));
wts[0] = this->centralWeight();
wts[1] = this->centralWeight();
// Reference point
point p0 = this->mesh().faceCentres()[facei];
// p0 -> p vector in the face-local coordinate system
vector d;
// Local coordinate scaling
scalar scale = 1;
// Matrix of the polynomial components
scalarRectangularMatrix B(C.size(), this->minSize(), scalar(0));
forAll(C, ip)
{
const point& p = C[ip];
const vector p0p = p - p0;
d.x() = p0p & idir;
d.y() = p0p & jdir;
d.z() = p0p & kdir;
if (ip == 0)
{
scale = cmptMax(cmptMag((d)));
}
// Scale the radius vector
d /= scale;
Polynomial::addCoeffs(B[ip], d, wts[ip], this->dim());
}
// Additional weighting for constant and linear terms
for (label i = 0; i < B.m(); i++)
{
B(i, 0) *= wts[0];
B(i, 1) *= wts[0];
}
// Set the fit
label stencilSize = C.size();
coeffsi.setSize(stencilSize);
bool goodFit = false;
for (int iIt = 0; iIt < 8 && !goodFit; iIt++)
{
SVD svd(B, small);
scalarRectangularMatrix invB(svd.VSinvUt());
for (label i=0; i<stencilSize; i++)
{
coeffsi[i] = wts[1]*wts[i]*invB(1, i)/scale;
}
goodFit =
(
mag(wts[0]*wts[0]*invB(0, 0) - wLin)
< this->linearLimitFactor()*wLin)
&& (mag(wts[0]*wts[1]*invB(0, 1) - (1 - wLin)
) < this->linearLimitFactor()*(1 - wLin))
&& coeffsi[0] < 0 && coeffsi[1] > 0
&& mag(coeffsi[0] + deltaCoeff) < 0.5*deltaCoeff
&& mag(coeffsi[1] - deltaCoeff) < 0.5*deltaCoeff;
if (!goodFit)
{
// (not good fit so increase weight in the centre and weight
// for constant and linear terms)
WarningInFunction
<< "Cannot fit face " << facei << " iteration " << iIt
<< " with sum of weights " << sum(coeffsi) << nl
<< " Weights " << coeffsi << nl
<< " Linear weights " << wLin << " " << 1 - wLin << nl
<< " deltaCoeff " << deltaCoeff << nl
<< " sing vals " << svd.S() << nl
<< "Components of goodFit:\n"
<< " wts[0]*wts[0]*invB(0, 0) = "
<< wts[0]*wts[0]*invB(0, 0) << nl
<< " wts[0]*wts[1]*invB(0, 1) = "
<< wts[0]*wts[1]*invB(0, 1)
<< " dim = " << this->dim() << endl;
wts[0] *= 10;
wts[1] *= 10;
for (label j = 0; j < B.n(); j++)
{
B(0, j) *= 10;
B(1, j) *= 10;
}
for (label i = 0; i < B.m(); i++)
{
B(i, 0) *= 10;
B(i, 1) *= 10;
}
}
}
if (goodFit)
{
// Remove the uncorrected coefficients
coeffsi[0] += deltaCoeff;
coeffsi[1] -= deltaCoeff;
}
else
{
WarningInFunction
<< "Could not fit face " << facei
<< " Coefficients = " << coeffsi
<< ", reverting to uncorrected." << endl;
coeffsi = 0;
}
}
void
LowOrderBeamIntegration::getWeightsDeriv(int numSections, double L,
double dLdh, double *dwtsdh)
{
for (int i = 0; i < numSections; i++)
dwtsdh[i] = 0.0;
if (parameterID == 0)
return;
double dxcdh[10];
double dxfdh[10];
for (int i = 0; i < 10; i++) {
dxcdh[i] = 0.0;
dxfdh[i] = 0.0;
}
double oneOverL = 1.0/L;
if (parameterID < 10) // xf
dxfdh[parameterID-1] = oneOverL;
else if (parameterID < 20) // xc
dxcdh[parameterID-10-1] = oneOverL;
else if (parameterID < 30) // wc
dwtsdh[parameterID-20-1] = oneOverL;
int N = pts.Size();
int Nf = N-Nc;
if (Nf > 0) {
Vector R(Nf);
double sum = 0.0;
for (int j = 0; j < Nc; j++)
sum += dwtsdh[j];
R(0) = -sum;
for (int i = 1; i < Nf; i++) {
sum = 0.0;
for (int j = 0; j < Nf; j++)
sum += i*pow(pts(Nc+j),i-1)*dxfdh[j]*wts(Nc+j);
for (int j = 0; j < Nc; j++)
sum += i*pow(pts(j),i-1)*dxcdh[j]*wts(j);
for (int j = 0; j < Nc; j++)
sum += pow(pts(j),i)*dwtsdh[j];
R(i) = -sum;
}
Matrix J(Nf,Nf);
for (int i = 0; i < Nf; i++)
for (int j = 0; j < Nf; j++)
J(i,j) = pow(pts(Nc+j),i);
Vector dwfdh(Nf);
J.Solve(R,dwfdh);
for (int i = 0; i < Nf; i++)
dwtsdh[Nc+i] += dwfdh(i);
}
return;
}
/**
* Gauss-Hermite quadature.
* Computes a Gauss-Hermite quadrature formula with simple knots.
* \param _t array of abscissa
* \param _wts array of corresponding wights
*/
void gauss_hermite (const dvector& _t,const dvector& _wts)
//
// Purpose:
//
// computes a Gauss quadrature formula with simple knots.
//
// Discussion:
//
// This routine computes all the knots and weights of a Gauss quadrature
// formula with a classical weight function with default values for A and B,
// and only simple knots.
//
// There are no moments checks and no printing is done.
//
// Use routine EIQFS to evaluate a quadrature computed by CGQFS.
//
// Licensing:
//
// This code is distributed under the GNU LGPL license.
//
// Modified:
//
// September 2010 by Derek Seiple
//
// Author:
//
// Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
// C++ version by John Burkardt.
//
// Reference:
//
// Sylvan Elhay, Jaroslav Kautsky,
// Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
// Interpolatory Quadrature,
// ACM Transactions on Mathematical Software,
// Volume 13, Number 4, December 1987, pages 399-415.
//
// Parameters:
//
// Output, double T[NT], the knots.
//
// Output, double WTS[NT], the weights.
//
{
dvector t=(dvector&) _t;
dvector wts=(dvector&) _wts;
if( t.indexmax()!=wts.indexmax() )
{
cerr << "Incompatible sizes in void "
<< "void gauss_hermite (const dvector& _t,const dvector& _wts)" << endl;
ad_exit(-1);
}
int lb = t.indexmin();
int ub = t.indexmax();
dvector aj(lb,ub);
dvector bj(lb,ub);
double zemu;
int i;
// Get the Jacobi matrix and zero-th moment.
zemu = 1.772453850905516;
for ( i = lb; i <= ub; i++ )
{
aj(i) = 0.0;
}
for ( i = lb; i <= ub; i++ )
{
bj(i) = sqrt((i-lb+1)/2.0);
}
// Compute the knots and weights.
if ( zemu <= 0.0 ) // Exit if the zero-th moment is not positive.
{
cout << "\n";
cout << "SGQF - Fatal error!\n";
cout << " ZEMU <= 0.\n";
exit ( 1 );
}
// Set up vectors for IMTQLX.
for ( i = lb; i <= ub; i++ )
{
t(i) = aj(i);
}
wts(lb) = sqrt ( zemu );
for ( i = lb+1; i <= ub; i++ )
{
wts(i) = 0.0;
}
// Diagonalize the Jacobi matrix.
imtqlx ( t, bj, wts );
for ( i = lb; i <= ub; i++ )
{
wts(i) = wts(i) * wts(i);
}
return;
}
const XC::Matrix &XC::DispBeamColumn2d::getTangentStiff(void) const
{
static Matrix kb(3,3);
this->getBasicStiff(kb);
// Zero for integral
q.Zero();
const size_t numSections= getNumSections();
const Matrix &pts = quadRule.getIntegrPointCoords(numSections);
const Vector &wts = quadRule.getIntegrPointWeights(numSections);
const double L = theCoordTransf->getInitialLength();
const double oneOverL = 1.0/L;
// Loop over the integration points
for(size_t i = 0; i < numSections; i++)
{
int order = theSections[i]->getOrder();
const XC::ID &code = theSections[i]->getType();
Matrix ka(workArea, order, 3);
ka.Zero();
double xi6 = 6.0*pts(i,0);
// Get the section tangent stiffness and stress resultant
const XC::Matrix &ks = theSections[i]->getSectionTangent();
const XC::Vector &s = theSections[i]->getStressResultant();
// Perform numerical integration
//kb.addMatrixTripleProduct(1.0, *B, ks, wts(i)/L);
double wti = wts(i)*oneOverL;
double tmp;
int j, k;
for(j = 0; j < order; j++) {
switch(code(j)) {
case SECTION_RESPONSE_P:
for(k = 0; k < order; k++)
ka(k,0) += ks(k,j)*wti;
break;
case SECTION_RESPONSE_MZ:
for(k = 0; k < order; k++) {
tmp = ks(k,j)*wti;
ka(k,1) += (xi6-4.0)*tmp;
ka(k,2) += (xi6-2.0)*tmp;
}
break;
default:
break;
}
}
for(j = 0; j < order; j++) {
switch (code(j)) {
case SECTION_RESPONSE_P:
for(k = 0; k < 3; k++)
kb(0,k) += ka(j,k);
break;
case SECTION_RESPONSE_MZ:
for(k = 0; k < 3; k++) {
tmp = ka(j,k);
kb(1,k) += (xi6-4.0)*tmp;
kb(2,k) += (xi6-2.0)*tmp;
}
break;
default:
break;
}
}
//q.addMatrixTransposeVector(1.0, *B, s, wts(i));
double si;
for(j = 0; j < order; j++)
{
si = s(j)*wts(i);
switch(code(j)) {
case SECTION_RESPONSE_P:
q(0) += si; break;
case SECTION_RESPONSE_MZ:
q(1) += (xi6-4.0)*si; q(2) += (xi6-2.0)*si; break;
default:
break;
}
}
}
// Add effects of element loads, q = q(v) + q0
q(0) += q0[0];
q(1) += q0[1];
q(2) += q0[2];
// Transform to global stiffness
K = theCoordTransf->getGlobalStiffMatrix(kb, q);
if(isDead())
K*=dead_srf;
return K;
}
