void goToBasePoint(){
for(int i = 0; i < 1000; i++){
stabilise(mainMotorId, basePosition, basicFeedbackCoefficient);
stabilise(clawMotorId, positionClawRelease, basicFeedbackCoefficient);
wait1Msec(1);
}
}
task stabilisation(){
while(1){
stabilise(mainMotorId, curentPosition, currentFeedbackCoefficient);
stabilise(clawMotorId, curentClawPosition, currentFeedbackCoefficient);
wait1Msec(1);
}
}
tmp<DimensionedField<scalar, GeoMesh> > stabilise
(
const DimensionedField<scalar, GeoMesh>& dsf,
const dimensioned<scalar>& ds
)
{
tmp<DimensionedField<scalar, GeoMesh> > tRes
(
new DimensionedField<scalar, GeoMesh>
(
IOobject
(
"stabilise(" + dsf.name() + ',' + ds.name() + ')',
dsf.instance(),
dsf.db()
),
dsf.mesh(),
dsf.dimensions() + ds.dimensions()
)
);
stabilise(tRes().getField(), dsf.getField(), ds.value());
return tRes;
}
tmp<scalarField> stabilise(const tmp<scalarField>& tsf, const scalar s)
{
tmp<scalarField> tRes = reuseTmp<scalar, scalar>::New(tsf);
stabilise(tRes(), tsf(), s);
reuseTmp<scalar, scalar>::clear(tsf);
return tRes;
}
// Cutting point for plane and line defined by origin and direction
Foam::scalar Foam::plane::normalIntersect
(
const point& pnt0,
const vector& dir
) const
{
scalar denom = stabilise((dir & unitVector_), VSMALL);
return ((basePoint_ - pnt0) & unitVector_)/denom;
}
void SOIL::check(boolean verbose = true) { //Detecting the water level by reading the analog value. Useful as Oriental level of water level
if(lastChange + period/2 <= millis()) {
on();
stabilise(); //pause for voltage stabilization
int analog = returnValue();
if(verbose) {
Serial.print("Analog value: ");
Serial.println(analog);
}
off();
lastChange = millis();
}
}
tmp<DimensionedField<scalar, GeoMesh> > stabilise
(
const tmp<DimensionedField<scalar, GeoMesh> >& tdsf,
const dimensioned<scalar>& ds
)
{
const DimensionedField<scalar, GeoMesh>& dsf = tdsf();
tmp<DimensionedField<scalar, GeoMesh> > tRes =
reuseTmpDimensionedField<scalar, scalar, GeoMesh>::New
(
tdsf,
"stabilise(" + dsf.name() + ',' + ds.name() + ')',
dsf.dimensions() + ds.dimensions()
);
stabilise(tRes().getField(), dsf.getField(), ds.value());
reuseTmpDimensionedField<scalar, scalar, GeoMesh>::clear(tdsf);
return tRes;
}
typename Foam::SolverPerformance<Type>
Foam::PCICG<Type, DType, LUType>::solve(Field<Type>& psi) const
{
word preconditionerName(this->controlDict_.lookup("preconditioner"));
// --- Setup class containing solver performance data
SolverPerformance<Type> solverPerf
(
preconditionerName + typeName,
this->fieldName_
);
label nCells = psi.size();
Type* __restrict__ psiPtr = psi.begin();
Field<Type> pA(nCells);
Type* __restrict__ pAPtr = pA.begin();
Field<Type> wA(nCells);
Type* __restrict__ wAPtr = wA.begin();
Type wArA = solverPerf.great_*pTraits<Type>::one;
Type wArAold = wArA;
// --- Calculate A.psi
this->matrix_.Amul(wA, psi);
// --- Calculate initial residual field
Field<Type> rA(this->matrix_.source() - wA);
Type* __restrict__ rAPtr = rA.begin();
// --- Calculate normalisation factor
Type normFactor = this->normFactor(psi, wA, pA);
if (LduMatrix<Type, DType, LUType>::debug >= 2)
{
Info<< " Normalisation factor = " << normFactor << endl;
}
// --- Calculate normalised residual norm
solverPerf.initialResidual() = cmptDivide(gSumCmptMag(rA), normFactor);
solverPerf.finalResidual() = solverPerf.initialResidual();
// --- Check convergence, solve if not converged
if
(
this->minIter_ > 0
|| !solverPerf.checkConvergence(this->tolerance_, this->relTol_)
)
{
// --- Select and construct the preconditioner
autoPtr<typename LduMatrix<Type, DType, LUType>::preconditioner>
preconPtr = LduMatrix<Type, DType, LUType>::preconditioner::New
(
*this,
this->controlDict_
);
// --- Solver iteration
do
{
// --- Store previous wArA
wArAold = wArA;
// --- Precondition residual
preconPtr->precondition(wA, rA);
// --- Update search directions:
wArA = gSumCmptProd(wA, rA);
if (solverPerf.nIterations() == 0)
{
for (label cell=0; cell<nCells; cell++)
{
pAPtr[cell] = wAPtr[cell];
}
}
else
{
Type beta = cmptDivide
(
wArA,
stabilise(wArAold, solverPerf.vsmall_)
);
for (label cell=0; cell<nCells; cell++)
{
pAPtr[cell] = wAPtr[cell] + cmptMultiply(beta, pAPtr[cell]);
}
}
// --- Update preconditioned residual
this->matrix_.Amul(wA, pA);
Type wApA = gSumCmptProd(wA, pA);
// --- Test for singularity
if
(
solverPerf.checkSingularity
(
cmptDivide(cmptMag(wApA), normFactor)
)
)
{
break;
}
// --- Update solution and residual:
Type alpha = cmptDivide
(
wArA,
stabilise(wApA, solverPerf.vsmall_)
);
for (label cell=0; cell<nCells; cell++)
{
psiPtr[cell] += cmptMultiply(alpha, pAPtr[cell]);
rAPtr[cell] -= cmptMultiply(alpha, wAPtr[cell]);
}
solverPerf.finalResidual() =
cmptDivide(gSumCmptMag(rA), normFactor);
} while
(
(
solverPerf.nIterations()++ < this->maxIter_
&& !solverPerf.checkConvergence(this->tolerance_, this->relTol_)
)
|| solverPerf.nIterations() < this->minIter_
);
}
return solverPerf;
}
//- Calculate inverse-distance weights for interpolative mapping
void topoCellMapper::calcInverseDistanceWeights() const
{
if (weightsPtr_)
{
FatalErrorIn
(
"void topoCellMapper::calcInverseDistanceWeights() const"
)
<< "Weights already calculated."
<< abort(FatalError);
}
// Fetch interpolative addressing
const labelListList& addr = addressing();
// Allocate memory
weightsPtr_ = new scalarListList(size());
scalarListList& w = *weightsPtr_;
// Obtain cell-centre information from old/new meshes
const vectorField& oldCentres = tMapper_.internalCentres();
const vectorField& newCentres = mesh_.cellCentres();
forAll(addr, cellI)
{
const labelList& mo = addr[cellI];
// Do mapped cells
if (mo.size() == 1)
{
w[cellI] = scalarList(1, 1.0);
}
else
{
// Map from masters, inverse-distance weights
scalar totalWeight = 0.0;
w[cellI] = scalarList(mo.size(), 0.0);
forAll (mo, oldCellI)
{
w[cellI][oldCellI] =
(
1.0/stabilise
(
magSqr
(
newCentres[cellI]
- oldCentres[mo[oldCellI]]
),
VSMALL
)
);
totalWeight += w[cellI][oldCellI];
}
// Normalize weights
scalar normFactor = (1.0/totalWeight);
forAll (mo, oldCellI)
{
w[cellI][oldCellI] *= normFactor;
}
}
}
tmp<scalarField> stabilise(const UList<scalar>& sf, const scalar s)
{
tmp<scalarField> tRes(new scalarField(sf.size()));
stabilise(tRes(), sf, s);
return tRes;
}
cmpt
);
scalar scalingFactorNum = 0.0;
scalar scalingFactorDenom = 0.0;
forAll(field, i)
{
scalingFactorNum += source[i]*field[i];
scalingFactorDenom += Acf[i]*field[i];
}
vector2D scalingVector(scalingFactorNum, scalingFactorDenom);
A.mesh().reduce(scalingVector, sumOp<vector2D>());
const scalar sf = scalingVector.x()/stabilise(scalingVector.y(), vSmall);
if (debug >= 2)
{
Pout<< sf << " ";
}
const scalarField& D = A.diag();
forAll(field, i)
{
field[i] = sf*field[i] + (source[i] - sf*Acf[i])/D[i];
}
}
task stabilisation(){
while(1){
stabilise(curentPosition, currentFeedbackCoefficient);
wait1Msec(1);
}
}
void goToBasePoint(){
for(int i = 0; i < 1000; i++){
stabilise(basePosition, basicFeedbackCoefficient);
wait1Msec(1);
}
}
void Foam::GAMGSolver::scale
(
scalargpuField& field,
scalargpuField& Acf,
const lduMatrix& A,
const FieldField<gpuField, scalar>& interfaceLevelBouCoeffs,
const lduInterfaceFieldPtrsList& interfaceLevel,
const scalargpuField& source,
const direction cmpt
) const
{
A.Amul
(
Acf,
field,
interfaceLevelBouCoeffs,
interfaceLevel,
cmpt
);
scalar scalingFactorNum = 0.0;
scalar scalingFactorDenom = 0.0;
scalingFactorNum =
thrust::reduce
(
thrust::make_transform_iterator
(
thrust::make_zip_iterator(thrust::make_tuple
(
source.begin(),
field.begin()
)),
multiplyTupleFunctor()
),
thrust::make_transform_iterator
(
thrust::make_zip_iterator(thrust::make_tuple
(
source.end(),
field.end()
)),
multiplyTupleFunctor()
),
0.0,
thrust::plus<scalar>()
);
scalingFactorDenom =
thrust::reduce
(
thrust::make_transform_iterator
(
thrust::make_zip_iterator(thrust::make_tuple
(
Acf.begin(),
field.begin()
)),
multiplyTupleFunctor()
),
thrust::make_transform_iterator
(
thrust::make_zip_iterator(thrust::make_tuple
(
Acf.end(),
field.end()
)),
multiplyTupleFunctor()
),
0.0,
thrust::plus<scalar>()
);
/*
forAll(field, i)
{
scalingFactorNum += source[i]*field[i];
scalingFactorDenom += Acf[i]*field[i];
}
*/
vector2D scalingVector(scalingFactorNum, scalingFactorDenom);
A.mesh().reduce(scalingVector, sumOp<vector2D>());
scalar sf = scalingVector.x()/stabilise(scalingVector.y(), VSMALL);
if (debug >= 2)
{
Pout<< sf << " ";
}
const scalargpuField& D = A.diag();
/*
forAll(field, i)
{
field[i] = sf*field[i] + (source[i] - sf*Acf[i])/D[i];
}
*/
thrust::transform
(
field.begin(),
field.end(),
thrust::make_zip_iterator(thrust::make_tuple
(
source.begin(),
Acf.begin(),
D.begin()
)),
field.begin(),
GAMGSolverScaleFunctor(sf)
);
}
Foam::SolverPerformance<Type>
Foam::PBiCICG<Type, DType, LUType>::solve(gpuField<Type>& psi) const
{
word preconditionerName(this->controlDict_.lookup("preconditioner"));
if(preconditionerName != "none")
preconditionerName = "diagonal";
// --- Setup class containing solver performance data
SolverPerformance<Type> solverPerf
(
preconditionerName + typeName,
this->fieldName_
);
register label nCells = psi.size();
gpuField<Type> pA(nCells);
gpuField<Type> pT(nCells, pTraits<Type>::zero);
gpuField<Type> wA(nCells);
gpuField<Type> wT(nCells);
Type wArT = solverPerf.great_*pTraits<Type>::one;
Type wArTold = wArT;
// --- Calculate A.psi and T.psi
this->matrix_.Amul(wA, psi);
this->matrix_.Tmul(wT, psi);
// --- Calculate initial residual and transpose residual fields
gpuField<Type> rA(this->matrix_.source() - wA);
gpuField<Type> rT(this->matrix_.source() - wT);
// --- Calculate normalisation factor
Type normFactor = this->normFactor(psi, wA, pA);
if (LduMatrix<Type, DType, LUType>::debug >= 2)
{
Info<< " Normalisation factor = " << normFactor << endl;
}
// --- Calculate normalised residual norm
solverPerf.initialResidual() = cmptDivide(gSumCmptMag(rA), normFactor);
solverPerf.finalResidual() = solverPerf.initialResidual();
// --- Check convergence, solve if not converged
if (!solverPerf.checkConvergence(this->tolerance_, this->relTol_))
{
// --- Select and construct the preconditioner
autoPtr<typename LduMatrix<Type, DType, LUType>::preconditioner>
preconPtr = LduMatrix<Type, DType, LUType>::preconditioner::New
(
*this,
this->controlDict_
);
// --- Solver iteration
do
{
// --- Store previous wArT
wArTold = wArT;
// --- Precondition residuals
preconPtr->precondition(wA, rA);
preconPtr->preconditionT(wT, rT);
// --- Update search directions:
wArT = gSumCmptProd(wA, rT);
if (solverPerf.nIterations() == 0)
{
thrust::copy(wA.begin(),wA.end(),pA.begin());
thrust::copy(wT.begin(),wT.end(),pT.begin());
}
else
{
Type beta = cmptDivide
(
wArT,
stabilise(wArTold, solverPerf.vsmall_)
);
thrust::transform
(
wA.begin(),
wA.end(),
thrust::make_transform_iterator
(
pA.begin(),
cmptMultiplyBinaryFunctionSFFunctor<Type,Type,Type>(beta)
),
pA.begin(),
addOperatorFunctor<Type,Type,Type>()
);
thrust::transform
(
wT.begin(),
wT.end(),
thrust::make_transform_iterator
(
pT.begin(),
cmptMultiplyBinaryFunctionSFFunctor<Type,Type,Type>(beta)
),
pT.begin(),
addOperatorFunctor<Type,Type,Type>()
);
}
// --- Update preconditioned residuals
this->matrix_.Amul(wA, pA);
this->matrix_.Tmul(wT, pT);
Type wApT = gSumCmptProd(wA, pT);
// --- Test for singularity
if
(
solverPerf.checkSingularity
(
cmptDivide(cmptMag(wApT), normFactor)
)
)
{
break;
}
// --- Update solution and residual:
Type alpha = cmptDivide
(
wArT,
stabilise(wApT, solverPerf.vsmall_)
);
thrust::transform
(
psi.begin(),
psi.end(),
thrust::make_transform_iterator
(
pA.begin(),
cmptMultiplyBinaryFunctionSFFunctor<Type,Type,Type>(alpha)
),
psi.begin(),
addOperatorFunctor<Type,Type,Type>()
);
thrust::transform
(
rA.begin(),
rA.end(),
thrust::make_transform_iterator
(
wA.begin(),
cmptMultiplyBinaryFunctionSFFunctor<Type,Type,Type>(alpha)
),
rA.begin(),
subtractOperatorFunctor<Type,Type,Type>()
);
thrust::transform
(
rT.begin(),
rT.end(),
thrust::make_transform_iterator
(
wT.begin(),
cmptMultiplyBinaryFunctionSFFunctor<Type,Type,Type>(alpha)
),
rT.begin(),
subtractOperatorFunctor<Type,Type,Type>()
);
solverPerf.finalResidual() =
cmptDivide(gSumCmptMag(rA), normFactor);
} while
(
solverPerf.nIterations()++ < this->maxIter_
&& !(solverPerf.checkConvergence(this->tolerance_, this->relTol_))
);
}
return solverPerf;
}
