order_result_t trash_water_treatment(void){
ROME_LOG(&rome_paddock,INFO,"Trashing water in treatment area");
order_result_t or;
set_speed(RS_FAST);
_wait_meca_ready();
ROME_SENDWAIT_MECA_CMD(&rome_meca,ROME_ENUM_MECA_COMMAND_PREPARE_TRASH_TREATMENT);
_wait_meca_ground_clear();
//store nb balls for scoring purpose
uint8_t balls_loaded = robot_state.cylinder_nb_bad;
//push away the cubes in front of treatment area
//the angle is changed to avoid pushing cubes towards recyling area
float angle = arfast(TEAM_SIDE_VALUE(ROBOT_SIDE_BACK, ROBOT_SIDE_BALLEATER), TABLE_SIDE_MAIN);
or = goto_pathfinding_node(PATHFINDING_GRAPH_NODE_MIDDLE_BOT, angle);
if (or != ORDER_SUCCESS)
return or;
//go in position to trash the bad water
or = goto_xya(KX(-250),250+130, arfast(ROBOT_SIDE_TURBINE,TABLE_SIDE_DOWN));
if (or != ORDER_SUCCESS)
return or;
_wait_meca_ready();
ROME_SENDWAIT_MECA_CMD(&rome_meca,ROME_ENUM_MECA_COMMAND_TRASH_TREATMENT);
_wait_meca_ground_clear();
update_score(10*balls_loaded);
or = goto_xya(KX(-250), 400, arfast(ROBOT_SIDE_TURBINE, TABLE_SIDE_DOWN));
set_speed(RS_NORMAL);
return or;
}
void strat_run(void)
{
ROME_LOG(&rome_paddock,INFO,"Go !!!");
ROME_SENDWAIT_ASSERV_GYRO_INTEGRATION(&rome_asserv, 1);
ROME_SENDWAIT_ASSERV_ACTIVATE(&rome_asserv, 1);
order_result_t or_take_water_near = ORDER_FAILURE;
order_result_t or_empty_water_near = ORDER_FAILURE;
order_result_t or_take_water_far = ORDER_FAILURE;
order_result_t or_empty_water_far = ORDER_FAILURE;
while (!( or_take_water_near == ORDER_SUCCESS &&
or_take_water_far == ORDER_SUCCESS &&
or_empty_water_near == ORDER_SUCCESS &&
or_empty_water_far == ORDER_SUCCESS ) ){
idle();
if (or_take_water_near != ORDER_SUCCESS){
or_take_water_near = take_water(DISPENSER_NEAR);
or_empty_water_near = empty_cylinder();
}
//go to the middle to reset the Y axis
if (goto_pathfinding_node(PATHFINDING_GRAPH_NODE_MIDDLE_BOT, arfast(ROBOT_SIDE_BACK,TABLE_SIDE_DOWN))
== ORDER_SUCCESS){
goto_xya(KX(0), 500, arfast(ROBOT_SIDE_BACK, TABLE_SIDE_DOWN));
autoset(ROBOT_SIDE_BACK, AUTOSET_DOWN, 0, 250+AUTOSET_OFFSET);
goto_xya(KX(0), 550, arfast(ROBOT_SIDE_BACK, TABLE_SIDE_DOWN));
}
//if oponent blocked us on the path to the middle, go back near starting area
else {
goto_pathfinding_node(PATHFINDING_GRAPH_NODE_WATER_TOWER, arfast(ROBOT_SIDE_BACK, TABLE_SIDE_DOWN));
}
if (or_take_water_far != ORDER_SUCCESS){
or_take_water_far = take_water(DISPENSER_FAR);
or_empty_water_far = empty_cylinder();
}
}
ROME_LOG(&rome_paddock,INFO,"That's all folks !");
idle_delay_ms(3000);
ROME_SENDWAIT_ASSERV_ACTIVATE(&rome_asserv, 0);
ROME_SENDWAIT_MECA_SET_POWER(&rome_meca, 0);
}
void strat_test(void)
{
ROME_LOG(&rome_paddock,INFO,"Strat test stuff");
for(;;) {
idle();
if(!robot_state.gyro_calibration)
break;
}
set_speed(RS_NORMAL);
#if 0
set_xya_wait(0,0,0);
goto_xya(0,0,0);
ROME_SENDWAIT_ASSERV_ACTIVATE(&rome_asserv, 1);
ROME_SENDWAIT_ASSERV_GYRO_INTEGRATION(&rome_asserv, 1);
ROME_LOG(&rome_paddock,INFO,"go");
goto_xya(0,700,0);
ROME_LOG(&rome_paddock,INFO,"back");
goto_xya(0,0,0);
#else
ROME_SENDWAIT_ASSERV_ACTIVATE(&rome_asserv, 1);
ROME_SENDWAIT_ASSERV_GYRO_INTEGRATION(&rome_asserv, 1);
ROME_LOG(&rome_paddock,INFO,"go");
goto_pathfinding_node(PATHFINDING_GRAPH_NODE_WATER_DISPENSER_FAR,
arfast(ROBOT_SIDE_BALLEATER,TABLE_SIDE_DOWN));
idle_delay_ms(5000);
ROME_LOG(&rome_paddock,INFO,"back");
goto_pathfinding_node(PATHFINDING_GRAPH_NODE_ORANGE_WATER_TOWER,
arfast(ROBOT_SIDE_BACK,TABLE_SIDE_MAIN));
ROME_LOG(&rome_paddock,INFO,"align to start a new test");
goto_xya(KX(1300), 1500, arfast(ROBOT_SIDE_BACK,TABLE_SIDE_MAIN));
#endif
ROME_LOG(&rome_paddock,INFO,"Strat test stuff end ...");
for(;;) {
idle();
}
}
void strat_prepare(void)
{
//initalise kx factor
robot_kx = TEAM_SIDE_VALUE(-1, 1);
//send color to meca
ROME_SENDWAIT_MECA_SET_ROBOT_COLOR(&rome_meca, robot_state.team == TEAM_GREEN);
ROME_LOG(&rome_paddock,DEBUG,"Strat prepare");
// initialize asserv
ROME_SENDWAIT_ASSERV_ACTIVATE(&rome_asserv, 1);
set_xya_wait(0,0,0);
ROME_SENDWAIT_ASSERV_GOTO_XY(&rome_asserv, 0, 0, 0);
ROME_SENDWAIT_ASSERV_SET_HTRAJ_XY_STEERING(&rome_asserv, 1.5, 0.03);
ROME_SENDWAIT_ASSERV_SET_HTRAJ_XY_CRUISE(&rome_asserv, 15, 0.03);
// autoset robot
// x in starting area
set_xya_wait(KX(0), 0, arfast(ROBOT_SIDE_BACK,TABLE_SIDE_MAIN));
autoset(ROBOT_SIDE_BACK,AUTOSET_MAIN, KX(1500-AUTOSET_OFFSET), 0);
goto_xya(KX(1000), 0, arfast(ROBOT_SIDE_BACK,TABLE_SIDE_MAIN));
// y on building area
goto_xya(KX(800), 400, arfast(ROBOT_SIDE_BACK,TABLE_SIDE_UP));
autoset(ROBOT_SIDE_BACK,AUTOSET_UP, 0, 2000-AUTOSET_OFFSET);
// check the state of the cylinder
_wait_meca_ready();
ROME_SENDWAIT_MECA_CMD(&rome_meca,ROME_ENUM_MECA_COMMAND_CHECK_EMPTY);
//evade the border, leaving space for galipette
goto_xya(KX(900), 1600, arfast(ROBOT_SIDE_BACK,TABLE_SIDE_UP));
//go in front of starting area
goto_xya(KX(1300), 1500, arfast(ROBOT_SIDE_BALLEATER,TABLE_SIDE_DOWN));
//wait for meca to end checking cylinder
_wait_meca_ready();
if (!cylinder_is_empty()){
_wait_meca_ready();
ROME_SENDWAIT_MECA_CMD(&rome_meca,ROME_ENUM_MECA_COMMAND_TRASH_BEGINMATCH);
}
//wait for meca to end all orders before shutting down asserv
_wait_meca_ready();
ROME_SENDWAIT_ASSERV_ACTIVATE(&rome_asserv, 0);
ROME_SENDWAIT_ASSERV_GYRO_INTEGRATION(&rome_asserv, 0);
}
order_result_t throw_water_watertower(void){
ROME_LOG(&rome_paddock,INFO,"Throwing water in watertower");
order_result_t or;
set_speed(RS_FAST);
_wait_meca_ready();
ROME_SENDWAIT_MECA_CMD(&rome_meca,ROME_ENUM_MECA_COMMAND_PREPARE_THROW_WATERTOWER);
_wait_meca_ground_clear();
uint8_t balls_loaded = robot_state.cylinder_nb_good;
or = goto_pathfinding_node(PATHFINDING_GRAPH_NODE_WATER_DISPENSER_NEAR,arfast(ROBOT_SIDE_TURBINE, TABLE_SIDE_MAIN));
if (or != ORDER_SUCCESS)
return or;
or = goto_xya(KX(1100), 1450, compute_throw_angle(1100,1450));
ROME_SENDWAIT_MECA_SET_THROW_POWER(&rome_meca,1950);
_wait_meca_ready();
ROME_SENDWAIT_MECA_CMD(&rome_meca,ROME_ENUM_MECA_COMMAND_THROW_WATERTOWER);
_wait_meca_ground_clear();
update_score(5*balls_loaded);
set_speed(RS_NORMAL);
return or;
}
int BlockDACG::reSolve(int numEigen, Epetra_MultiVector &Q, double *lambda, int startingEV) {
// Computes the smallest eigenvalues and the corresponding eigenvectors
// of the generalized eigenvalue problem
//
// K X = M X Lambda
//
// using a Block Deflation Accelerated Conjugate Gradient algorithm.
//
// Note that if M is not specified, then K X = X Lambda is solved.
//
// Ref: P. Arbenz & R. Lehoucq, "A comparison of algorithms for modal analysis in the
// absence of a sparse direct method", SNL, Technical Report SAND2003-1028J
// With the notations of this report, the coefficient beta is defined as
// diag( H^T_{k} G_{k} ) / diag( H^T_{k-1} G_{k-1} )
//
// Input variables:
//
// numEigen (integer) = Number of eigenmodes requested
//
// Q (Epetra_MultiVector) = Converged eigenvectors
// The number of columns of Q must be equal to numEigen + blockSize.
// The rows of Q are distributed across processors.
// At exit, the first numEigen columns contain the eigenvectors requested.
//
// lambda (array of doubles) = Converged eigenvalues
// At input, it must be of size numEigen + blockSize.
// At exit, the first numEigen locations contain the eigenvalues requested.
//
// startingEV (integer) = Number of existing converged eigenmodes
//
// Return information on status of computation
//
// info >= 0 >> Number of converged eigenpairs at the end of computation
//
// // Failure due to input arguments
//
// info = - 1 >> The stiffness matrix K has not been specified.
// info = - 2 >> The maps for the matrix K and the matrix M differ.
// info = - 3 >> The maps for the matrix K and the preconditioner P differ.
// info = - 4 >> The maps for the vectors and the matrix K differ.
// info = - 5 >> Q is too small for the number of eigenvalues requested.
// info = - 6 >> Q is too small for the computation parameters.
//
// info = - 10 >> Failure during the mass orthonormalization
//
// info = - 20 >> Error in LAPACK during the local eigensolve
//
// info = - 30 >> MEMORY
//
// Check the input parameters
if (numEigen <= startingEV) {
return startingEV;
}
int info = myVerify.inputArguments(numEigen, K, M, Prec, Q, numEigen + blockSize);
if (info < 0)
return info;
int myPid = MyComm.MyPID();
// Get the weight for approximating the M-inverse norm
Epetra_Vector *vectWeight = 0;
if (normWeight) {
vectWeight = new Epetra_Vector(View, Q.Map(), normWeight);
}
int knownEV = startingEV;
int localVerbose = verbose*(myPid==0);
// Define local block vectors
//
// MX = Working vectors (storing M*X if M is specified, else pointing to X)
// KX = Working vectors (storing K*X)
//
// R = Residuals
//
// H = Preconditioned residuals
//
// P = Search directions
// MP = Working vectors (storing M*P if M is specified, else pointing to P)
// KP = Working vectors (storing K*P)
int xr = Q.MyLength();
Epetra_MultiVector X(View, Q, numEigen, blockSize);
X.Random();
int tmp;
tmp = (M == 0) ? 5*blockSize*xr : 7*blockSize*xr;
double *work1 = new (nothrow) double[tmp];
if (work1 == 0) {
if (vectWeight)
delete vectWeight;
info = -30;
return info;
}
memRequested += sizeof(double)*tmp/(1024.0*1024.0);
highMem = (highMem > currentSize()) ? highMem : currentSize();
double *tmpD = work1;
Epetra_MultiVector KX(View, Q.Map(), tmpD, xr, blockSize);
tmpD = tmpD + xr*blockSize;
Epetra_MultiVector MX(View, Q.Map(), (M) ? tmpD : X.Values(), xr, blockSize);
tmpD = (M) ? tmpD + xr*blockSize : tmpD;
Epetra_MultiVector R(View, Q.Map(), tmpD, xr, blockSize);
tmpD = tmpD + xr*blockSize;
Epetra_MultiVector H(View, Q.Map(), tmpD, xr, blockSize);
tmpD = tmpD + xr*blockSize;
Epetra_MultiVector P(View, Q.Map(), tmpD, xr, blockSize);
tmpD = tmpD + xr*blockSize;
Epetra_MultiVector KP(View, Q.Map(), tmpD, xr, blockSize);
tmpD = tmpD + xr*blockSize;
Epetra_MultiVector MP(View, Q.Map(), (M) ? tmpD : P.Values(), xr, blockSize);
// Define arrays
//
// theta = Store the local eigenvalues (size: 2*blockSize)
// normR = Store the norm of residuals (size: blockSize)
//
// oldHtR = Store the previous H_i^T*R_i (size: blockSize)
// currentHtR = Store the current H_i^T*R_i (size: blockSize)
//
// MM = Local mass matrix (size: 2*blockSize x 2*blockSize)
// KK = Local stiffness matrix (size: 2*blockSize x 2*blockSize)
//
// S = Local eigenvectors (size: 2*blockSize x 2*blockSize)
int lwork2;
lwork2 = 5*blockSize + 12*blockSize*blockSize;
double *work2 = new (nothrow) double[lwork2];
if (work2 == 0) {
if (vectWeight)
delete vectWeight;
delete[] work1;
info = -30;
return info;
}
highMem = (highMem > currentSize()) ? highMem : currentSize();
tmpD = work2;
double *theta = tmpD;
tmpD = tmpD + 2*blockSize;
double *normR = tmpD;
tmpD = tmpD + blockSize;
double *oldHtR = tmpD;
tmpD = tmpD + blockSize;
double *currentHtR = tmpD;
tmpD = tmpD + blockSize;
memset(currentHtR, 0, blockSize*sizeof(double));
double *MM = tmpD;
tmpD = tmpD + 4*blockSize*blockSize;
double *KK = tmpD;
tmpD = tmpD + 4*blockSize*blockSize;
double *S = tmpD;
memRequested += sizeof(double)*lwork2/(1024.0*1024.0);
// Define an array to store the residuals history
if (localVerbose > 2) {
resHistory = new (nothrow) double[maxIterEigenSolve*blockSize];
if (resHistory == 0) {
if (vectWeight)
delete vectWeight;
delete[] work1;
delete[] work2;
info = -30;
return info;
}
historyCount = 0;
}
// Miscellaneous definitions
bool reStart = false;
numRestart = 0;
int localSize;
int twoBlocks = 2*blockSize;
int nFound = blockSize;
int i, j;
if (localVerbose > 0) {
cout << endl;
cout << " *|* Problem: ";
if (M)
cout << "K*Q = M*Q D ";
else
cout << "K*Q = Q D ";
if (Prec)
cout << " with preconditioner";
cout << endl;
cout << " *|* Algorithm = DACG (block version)" << endl;
cout << " *|* Size of blocks = " << blockSize << endl;
cout << " *|* Number of requested eigenvalues = " << numEigen << endl;
cout.precision(2);
cout.setf(ios::scientific, ios::floatfield);
cout << " *|* Tolerance for convergence = " << tolEigenSolve << endl;
cout << " *|* Norm used for convergence: ";
if (normWeight)
cout << "weighted L2-norm with user-provided weights" << endl;
else
cout << "L^2-norm" << endl;
if (startingEV > 0)
cout << " *|* Input converged eigenvectors = " << startingEV << endl;
cout << "\n -- Start iterations -- \n";
}
timeOuterLoop -= MyWatch.WallTime();
for (outerIter = 1; outerIter <= maxIterEigenSolve; ++outerIter) {
highMem = (highMem > currentSize()) ? highMem : currentSize();
if ((outerIter == 1) || (reStart == true)) {
reStart = false;
localSize = blockSize;
if (nFound > 0) {
Epetra_MultiVector X2(View, X, blockSize-nFound, nFound);
Epetra_MultiVector MX2(View, MX, blockSize-nFound, nFound);
Epetra_MultiVector KX2(View, KX, blockSize-nFound, nFound);
// Apply the mass matrix to X
timeMassOp -= MyWatch.WallTime();
if (M)
M->Apply(X2, MX2);
timeMassOp += MyWatch.WallTime();
massOp += nFound;
if (knownEV > 0) {
// Orthonormalize X against the known eigenvectors with Gram-Schmidt
// Note: Use R as a temporary work space
Epetra_MultiVector copyQ(View, Q, 0, knownEV);
timeOrtho -= MyWatch.WallTime();
info = modalTool.massOrthonormalize(X, MX, M, copyQ, nFound, 0, R.Values());
timeOrtho += MyWatch.WallTime();
// Exit the code if the orthogonalization did not succeed
if (info < 0) {
info = -10;
delete[] work1;
delete[] work2;
if (vectWeight)
delete vectWeight;
return info;
}
}
// Apply the stiffness matrix to X
timeStifOp -= MyWatch.WallTime();
K->Apply(X2, KX2);
timeStifOp += MyWatch.WallTime();
stifOp += nFound;
} // if (nFound > 0)
} // if ((outerIter == 1) || (reStart == true))
else {
// Apply the preconditioner on the residuals
if (Prec != 0) {
timePrecOp -= MyWatch.WallTime();
Prec->ApplyInverse(R, H);
timePrecOp += MyWatch.WallTime();
precOp += blockSize;
}
else {
memcpy(H.Values(), R.Values(), xr*blockSize*sizeof(double));
}
// Compute the product H^T*R
timeSearchP -= MyWatch.WallTime();
memcpy(oldHtR, currentHtR, blockSize*sizeof(double));
H.Dot(R, currentHtR);
// Define the new search directions
if (localSize == blockSize) {
P.Scale(-1.0, H);
localSize = twoBlocks;
} // if (localSize == blockSize)
else {
bool hasZeroDot = false;
for (j = 0; j < blockSize; ++j) {
if (oldHtR[j] == 0.0) {
hasZeroDot = true;
break;
}
callBLAS.SCAL(xr, currentHtR[j]/oldHtR[j], P.Values() + j*xr);
}
if (hasZeroDot == true) {
// Restart the computation when there is a null dot product
if (localVerbose > 0) {
cout << endl;
cout << " !! Null dot product -- Restart the search space !!\n";
cout << endl;
}
if (blockSize == 1) {
X.Random();
nFound = blockSize;
}
else {
Epetra_MultiVector Xinit(View, X, j, blockSize-j);
Xinit.Random();
nFound = blockSize - j;
} // if (blockSize == 1)
reStart = true;
numRestart += 1;
info = 0;
continue;
}
callBLAS.AXPY(xr*blockSize, -1.0, H.Values(), P.Values());
} // if (localSize == blockSize)
timeSearchP += MyWatch.WallTime();
// Apply the mass matrix on P
timeMassOp -= MyWatch.WallTime();
if (M)
M->Apply(P, MP);
timeMassOp += MyWatch.WallTime();
massOp += blockSize;
if (knownEV > 0) {
// Orthogonalize P against the known eigenvectors
// Note: Use R as a temporary work space
Epetra_MultiVector copyQ(View, Q, 0, knownEV);
timeOrtho -= MyWatch.WallTime();
modalTool.massOrthonormalize(P, MP, M, copyQ, blockSize, 1, R.Values());
timeOrtho += MyWatch.WallTime();
}
// Apply the stiffness matrix to P
timeStifOp -= MyWatch.WallTime();
K->Apply(P, KP);
timeStifOp += MyWatch.WallTime();
stifOp += blockSize;
} // if ((outerIter == 1) || (reStart == true))
// Form "local" mass and stiffness matrices
// Note: Use S as a temporary workspace
timeLocalProj -= MyWatch.WallTime();
modalTool.localProjection(blockSize, blockSize, xr, X.Values(), xr, KX.Values(), xr,
KK, localSize, S);
modalTool.localProjection(blockSize, blockSize, xr, X.Values(), xr, MX.Values(), xr,
MM, localSize, S);
if (localSize > blockSize) {
modalTool.localProjection(blockSize, blockSize, xr, X.Values(), xr, KP.Values(), xr,
KK + blockSize*localSize, localSize, S);
modalTool.localProjection(blockSize, blockSize, xr, P.Values(), xr, KP.Values(), xr,
KK + blockSize*localSize + blockSize, localSize, S);
modalTool.localProjection(blockSize, blockSize, xr, X.Values(), xr, MP.Values(), xr,
MM + blockSize*localSize, localSize, S);
modalTool.localProjection(blockSize, blockSize, xr, P.Values(), xr, MP.Values(), xr,
MM + blockSize*localSize + blockSize, localSize, S);
} // if (localSize > blockSize)
timeLocalProj += MyWatch.WallTime();
// Perform a spectral decomposition
timeLocalSolve -= MyWatch.WallTime();
int nevLocal = localSize;
info = modalTool.directSolver(localSize, KK, localSize, MM, localSize, nevLocal,
S, localSize, theta, localVerbose,
(blockSize == 1) ? 1: 0);
timeLocalSolve += MyWatch.WallTime();
if (info < 0) {
// Stop when spectral decomposition has a critical failure
break;
}
// Check for restarting
if ((theta[0] < 0.0) || (nevLocal < blockSize)) {
if (localVerbose > 0) {
cout << " Iteration " << outerIter;
cout << "- Failure for spectral decomposition - RESTART with new random search\n";
}
if (blockSize == 1) {
X.Random();
nFound = blockSize;
}
else {
Epetra_MultiVector Xinit(View, X, 1, blockSize-1);
Xinit.Random();
nFound = blockSize - 1;
} // if (blockSize == 1)
reStart = true;
numRestart += 1;
info = 0;
continue;
} // if ((theta[0] < 0.0) || (nevLocal < blockSize))
if ((localSize == twoBlocks) && (nevLocal == blockSize)) {
for (j = 0; j < nevLocal; ++j)
memcpy(S + j*blockSize, S + j*twoBlocks, blockSize*sizeof(double));
localSize = blockSize;
}
// Check the direction of eigenvectors
// Note: This sign check is important for convergence
for (j = 0; j < nevLocal; ++j) {
double coeff = S[j + j*localSize];
if (coeff < 0.0)
callBLAS.SCAL(localSize, -1.0, S + j*localSize);
}
// Compute the residuals
timeResidual -= MyWatch.WallTime();
callBLAS.GEMM('N', 'N', xr, blockSize, blockSize, 1.0, KX.Values(), xr,
S, localSize, 0.0, R.Values(), xr);
if (localSize == twoBlocks) {
callBLAS.GEMM('N', 'N', xr, blockSize, blockSize, 1.0, KP.Values(), xr,
S + blockSize, localSize, 1.0, R.Values(), xr);
}
for (j = 0; j < blockSize; ++j)
callBLAS.SCAL(localSize, theta[j], S + j*localSize);
callBLAS.GEMM('N', 'N', xr, blockSize, blockSize, -1.0, MX.Values(), xr,
S, localSize, 1.0, R.Values(), xr);
if (localSize == twoBlocks) {
callBLAS.GEMM('N', 'N', xr, blockSize, blockSize, -1.0, MP.Values(), xr,
S + blockSize, localSize, 1.0, R.Values(), xr);
}
for (j = 0; j < blockSize; ++j)
callBLAS.SCAL(localSize, 1.0/theta[j], S + j*localSize);
timeResidual += MyWatch.WallTime();
// Compute the norms of the residuals
timeNorm -= MyWatch.WallTime();
if (vectWeight)
R.NormWeighted(*vectWeight, normR);
else
R.Norm2(normR);
// Scale the norms of residuals with the eigenvalues
// Count the converged eigenvectors
nFound = 0;
for (j = 0; j < blockSize; ++j) {
normR[j] = (theta[j] == 0.0) ? normR[j] : normR[j]/theta[j];
if (normR[j] < tolEigenSolve)
nFound += 1;
}
timeNorm += MyWatch.WallTime();
// Store the residual history
if (localVerbose > 2) {
memcpy(resHistory + historyCount*blockSize, normR, blockSize*sizeof(double));
historyCount += 1;
}
// Print information on current iteration
if (localVerbose > 0) {
cout << " Iteration " << outerIter << " - Number of converged eigenvectors ";
cout << knownEV + nFound << endl;
}
if (localVerbose > 1) {
cout << endl;
cout.precision(2);
cout.setf(ios::scientific, ios::floatfield);
for (i=0; i<blockSize; ++i) {
cout << " Iteration " << outerIter << " - Scaled Norm of Residual " << i;
cout << " = " << normR[i] << endl;
}
cout << endl;
cout.precision(2);
for (i=0; i<blockSize; ++i) {
cout << " Iteration " << outerIter << " - Ritz eigenvalue " << i;
cout.setf((fabs(theta[i]) < 0.01) ? ios::scientific : ios::fixed, ios::floatfield);
cout << " = " << theta[i] << endl;
}
cout << endl;
}
if (nFound == 0) {
// Update the spaces
// Note: Use H as a temporary work space
timeLocalUpdate -= MyWatch.WallTime();
memcpy(H.Values(), X.Values(), xr*blockSize*sizeof(double));
callBLAS.GEMM('N', 'N', xr, blockSize, blockSize, 1.0, H.Values(), xr, S, localSize,
0.0, X.Values(), xr);
memcpy(H.Values(), KX.Values(), xr*blockSize*sizeof(double));
callBLAS.GEMM('N', 'N', xr, blockSize, blockSize, 1.0, H.Values(), xr, S, localSize,
0.0, KX.Values(), xr);
if (M) {
memcpy(H.Values(), MX.Values(), xr*blockSize*sizeof(double));
callBLAS.GEMM('N', 'N', xr, blockSize, blockSize, 1.0, H.Values(), xr, S, localSize,
0.0, MX.Values(), xr);
}
if (localSize == twoBlocks) {
callBLAS.GEMM('N', 'N', xr, blockSize, blockSize, 1.0, P.Values(), xr,
S + blockSize, localSize, 1.0, X.Values(), xr);
callBLAS.GEMM('N', 'N', xr, blockSize, blockSize, 1.0, KP.Values(), xr,
S + blockSize, localSize, 1.0, KX.Values(), xr);
if (M) {
callBLAS.GEMM('N', 'N', xr, blockSize, blockSize, 1.0, MP.Values(), xr,
S + blockSize, localSize, 1.0, MX.Values(), xr);
}
} // if (localSize == twoBlocks)
timeLocalUpdate += MyWatch.WallTime();
// When required, monitor some orthogonalities
if (verbose > 2) {
if (knownEV == 0) {
accuracyCheck(&X, &MX, &R, 0, (localSize>blockSize) ? &P : 0);
}
else {
Epetra_MultiVector copyQ(View, Q, 0, knownEV);
accuracyCheck(&X, &MX, &R, ©Q, (localSize>blockSize) ? &P : 0);
}
} // if (verbose > 2)
continue;
} // if (nFound == 0)
// Order the Ritz eigenvectors by putting the converged vectors at the beginning
int firstIndex = blockSize;
for (j = 0; j < blockSize; ++j) {
if (normR[j] >= tolEigenSolve) {
firstIndex = j;
break;
}
} // for (j = 0; j < blockSize; ++j)
while (firstIndex < nFound) {
for (j = firstIndex; j < blockSize; ++j) {
if (normR[j] < tolEigenSolve) {
// Swap the j-th and firstIndex-th position
callFortran.SWAP(localSize, S + j*localSize, 1, S + firstIndex*localSize, 1);
callFortran.SWAP(1, theta + j, 1, theta + firstIndex, 1);
callFortran.SWAP(1, normR + j, 1, normR + firstIndex, 1);
break;
}
} // for (j = firstIndex; j < blockSize; ++j)
for (j = 0; j < blockSize; ++j) {
if (normR[j] >= tolEigenSolve) {
firstIndex = j;
break;
}
} // for (j = 0; j < blockSize; ++j)
} // while (firstIndex < nFound)
// Copy the converged eigenvalues
memcpy(lambda + knownEV, theta, nFound*sizeof(double));
// Convergence test
if (knownEV + nFound >= numEigen) {
callBLAS.GEMM('N', 'N', xr, nFound, blockSize, 1.0, X.Values(), xr,
S, localSize, 0.0, R.Values(), xr);
if (localSize > blockSize) {
callBLAS.GEMM('N', 'N', xr, nFound, blockSize, 1.0, P.Values(), xr,
S + blockSize, localSize, 1.0, R.Values(), xr);
}
memcpy(Q.Values() + knownEV*xr, R.Values(), nFound*xr*sizeof(double));
knownEV += nFound;
if (localVerbose == 1) {
cout << endl;
cout.precision(2);
cout.setf(ios::scientific, ios::floatfield);
for (i=0; i<blockSize; ++i) {
cout << " Iteration " << outerIter << " - Scaled Norm of Residual " << i;
cout << " = " << normR[i] << endl;
}
cout << endl;
}
break;
}
// Store the converged eigenvalues and eigenvectors
callBLAS.GEMM('N', 'N', xr, nFound, blockSize, 1.0, X.Values(), xr,
S, localSize, 0.0, Q.Values() + knownEV*xr, xr);
if (localSize == twoBlocks) {
callBLAS.GEMM('N', 'N', xr, nFound, blockSize, 1.0, P.Values(), xr,
S + blockSize, localSize, 1.0, Q.Values() + knownEV*xr, xr);
}
knownEV += nFound;
// Define the restarting vectors
timeRestart -= MyWatch.WallTime();
int leftOver = (nevLocal < blockSize + nFound) ? nevLocal - nFound : blockSize;
double *Snew = S + nFound*localSize;
memcpy(H.Values(), X.Values(), blockSize*xr*sizeof(double));
callBLAS.GEMM('N', 'N', xr, leftOver, blockSize, 1.0, H.Values(), xr,
Snew, localSize, 0.0, X.Values(), xr);
memcpy(H.Values(), KX.Values(), blockSize*xr*sizeof(double));
callBLAS.GEMM('N', 'N', xr, leftOver, blockSize, 1.0, H.Values(), xr,
Snew, localSize, 0.0, KX.Values(), xr);
if (M) {
memcpy(H.Values(), MX.Values(), blockSize*xr*sizeof(double));
callBLAS.GEMM('N', 'N', xr, leftOver, blockSize, 1.0, H.Values(), xr,
Snew, localSize, 0.0, MX.Values(), xr);
}
if (localSize == twoBlocks) {
callBLAS.GEMM('N', 'N', xr, leftOver, blockSize, 1.0, P.Values(), xr,
Snew+blockSize, localSize, 1.0, X.Values(), xr);
callBLAS.GEMM('N', 'N', xr, leftOver, blockSize, 1.0, KP.Values(), xr,
Snew+blockSize, localSize, 1.0, KX.Values(), xr);
if (M) {
callBLAS.GEMM('N', 'N', xr, leftOver, blockSize, 1.0, MP.Values(), xr,
Snew+blockSize, localSize, 1.0, MX.Values(), xr);
}
} // if (localSize == twoBlocks)
if (nevLocal < blockSize + nFound) {
// Put new random vectors at the end of the block
Epetra_MultiVector Xtmp(View, X, leftOver, blockSize - leftOver);
Xtmp.Random();
}
else {
nFound = 0;
} // if (nevLocal < blockSize + nFound)
reStart = true;
timeRestart += MyWatch.WallTime();
} // for (outerIter = 1; outerIter <= maxIterEigenSolve; ++outerIter)
timeOuterLoop += MyWatch.WallTime();
highMem = (highMem > currentSize()) ? highMem : currentSize();
// Clean memory
delete[] work1;
delete[] work2;
if (vectWeight)
delete vectWeight;
// Sort the eigenpairs
timePostProce -= MyWatch.WallTime();
if ((info == 0) && (knownEV > 0)) {
mySort.sortScalars_Vectors(knownEV, lambda, Q.Values(), Q.MyLength());
}
timePostProce += MyWatch.WallTime();
return (info == 0) ? knownEV : info;
}
int Davidson::reSolve(int numEigen, Epetra_MultiVector &Q, double *lambda, int startingEV) {
// Computes the smallest eigenvalues and the corresponding eigenvectors
// of the generalized eigenvalue problem
//
// K X = M X Lambda
//
// using a generalized Davidson algorithm
//
// Note that if M is not specified, then K X = X Lambda is solved.
//
// Input variables:
//
// numEigen (integer) = Number of eigenmodes requested
//
// Q (Epetra_MultiVector) = Converged eigenvectors
// The number of columns of Q must be at least numEigen + blockSize.
// The rows of Q are distributed across processors.
// At exit, the first numEigen columns contain the eigenvectors requested.
//
// lambda (array of doubles) = Converged eigenvalues
// At input, it must be of size numEigen + blockSize.
// At exit, the first numEigen locations contain the eigenvalues requested.
//
// startingEV (integer) = Number of existing converged eigenvectors
// We assume that the user has check the eigenvectors and
// their M-orthonormality.
//
// Return information on status of computation
//
// info >= 0 >> Number of converged eigenpairs at the end of computation
//
// // Failure due to input arguments
//
// info = - 1 >> The stiffness matrix K has not been specified.
// info = - 2 >> The maps for the matrix K and the matrix M differ.
// info = - 3 >> The maps for the matrix K and the preconditioner P differ.
// info = - 4 >> The maps for the vectors and the matrix K differ.
// info = - 5 >> Q is too small for the number of eigenvalues requested.
// info = - 6 >> Q is too small for the computation parameters.
//
// info = - 8 >> The number of blocks is too small for the number of eigenvalues.
//
// info = - 10 >> Failure during the mass orthonormalization
//
// info = - 30 >> MEMORY
//
// Check the input parameters
if (numEigen <= startingEV) {
return startingEV;
}
int info = myVerify.inputArguments(numEigen, K, M, Prec, Q, minimumSpaceDimension(numEigen));
if (info < 0)
return info;
int myPid = MyComm.MyPID();
if (numBlock*blockSize < numEigen) {
if (myPid == 0) {
cerr << endl;
cerr << " !!! The space dimension (# of blocks x size of blocks) must be greater than ";
cerr << " the number of eigenvalues !!!\n";
cerr << " Number of blocks = " << numBlock << endl;
cerr << " Size of blocks = " << blockSize << endl;
cerr << " Number of eigenvalues = " << numEigen << endl;
cerr << endl;
}
return -8;
}
// Get the weight for approximating the M-inverse norm
Epetra_Vector *vectWeight = 0;
if (normWeight) {
vectWeight = new Epetra_Vector(View, Q.Map(), normWeight);
}
int knownEV = startingEV;
int localVerbose = verbose*(myPid==0);
// Define local block vectors
//
// MX = Working vectors (storing M*X if M is specified, else pointing to X)
// KX = Working vectors (storing K*X)
//
// R = Residuals
int xr = Q.MyLength();
int dimSearch = blockSize*numBlock;
Epetra_MultiVector X(View, Q, 0, dimSearch + blockSize);
if (knownEV > 0) {
Epetra_MultiVector copyX(View, Q, knownEV, blockSize);
copyX.Random();
}
else {
X.Random();
}
int tmp;
tmp = (M == 0) ? 2*blockSize*xr : 3*blockSize*xr;
double *work1 = new (nothrow) double[tmp];
if (work1 == 0) {
if (vectWeight)
delete vectWeight;
info = -30;
return info;
}
memRequested += sizeof(double)*tmp/(1024.0*1024.0);
highMem = (highMem > currentSize()) ? highMem : currentSize();
double *tmpD = work1;
Epetra_MultiVector KX(View, Q.Map(), tmpD, xr, blockSize);
tmpD = tmpD + xr*blockSize;
Epetra_MultiVector MX(View, Q.Map(), (M) ? tmpD : X.Values(), xr, blockSize);
tmpD = (M) ? tmpD + xr*blockSize : tmpD;
Epetra_MultiVector R(View, Q.Map(), tmpD, xr, blockSize);
// Define arrays
//
// theta = Store the local eigenvalues (size: dimSearch)
// normR = Store the norm of residuals (size: blockSize)
//
// KK = Local stiffness matrix (size: dimSearch x dimSearch)
//
// S = Local eigenvectors (size: dimSearch x dimSearch)
//
// tmpKK = Local workspace (size: blockSize x blockSize)
int lwork2 = blockSize + dimSearch + 2*dimSearch*dimSearch + blockSize*blockSize;
double *work2 = new (nothrow) double[lwork2];
if (work2 == 0) {
if (vectWeight)
delete vectWeight;
delete[] work1;
info = -30;
return info;
}
memRequested += sizeof(double)*lwork2/(1024.0*1024.0);
highMem = (highMem > currentSize()) ? highMem : currentSize();
tmpD = work2;
double *theta = tmpD;
tmpD = tmpD + dimSearch;
double *normR = tmpD;
tmpD = tmpD + blockSize;
double *KK = tmpD;
tmpD = tmpD + dimSearch*dimSearch;
memset(KK, 0, dimSearch*dimSearch*sizeof(double));
double *S = tmpD;
tmpD = tmpD + dimSearch*dimSearch;
double *tmpKK = tmpD;
// Define an array to store the residuals history
if (localVerbose > 2) {
resHistory = new (nothrow) double[maxIterEigenSolve*blockSize];
spaceSizeHistory = new (nothrow) int[maxIterEigenSolve];
if ((resHistory == 0) || (spaceSizeHistory == 0)) {
if (vectWeight)
delete vectWeight;
delete[] work1;
delete[] work2;
info = -30;
return info;
}
historyCount = 0;
}
// Miscellaneous definitions
bool reStart = false;
numRestart = 0;
bool criticalExit = false;
int bStart = 0;
int offSet = 0;
numBlock = (dimSearch/blockSize) - (knownEV/blockSize);
int nFound = blockSize;
int i, j;
if (localVerbose > 0) {
cout << endl;
cout << " *|* Problem: ";
if (M)
cout << "K*Q = M*Q D ";
else
cout << "K*Q = Q D ";
if (Prec)
cout << " with preconditioner";
cout << endl;
cout << " *|* Algorithm = Davidson algorithm (block version)" << endl;
cout << " *|* Size of blocks = " << blockSize << endl;
cout << " *|* Largest size of search space = " << numBlock*blockSize << endl;
cout << " *|* Number of requested eigenvalues = " << numEigen << endl;
cout.precision(2);
cout.setf(ios::scientific, ios::floatfield);
cout << " *|* Tolerance for convergence = " << tolEigenSolve << endl;
cout << " *|* Norm used for convergence: ";
if (vectWeight)
cout << "weighted L2-norm with user-provided weights" << endl;
else
cout << "L^2-norm" << endl;
if (startingEV > 0)
cout << " *|* Input converged eigenvectors = " << startingEV << endl;
cout << "\n -- Start iterations -- \n";
}
int maxBlock = (dimSearch/blockSize) - (knownEV/blockSize);
timeOuterLoop -= MyWatch.WallTime();
outerIter = 0;
while (outerIter <= maxIterEigenSolve) {
highMem = (highMem > currentSize()) ? highMem : currentSize();
int nb;
for (nb = bStart; nb < maxBlock; ++nb) {
outerIter += 1;
if (outerIter > maxIterEigenSolve)
break;
int localSize = nb*blockSize;
Epetra_MultiVector Xcurrent(View, X, localSize + knownEV, blockSize);
timeMassOp -= MyWatch.WallTime();
if (M)
M->Apply(Xcurrent, MX);
timeMassOp += MyWatch.WallTime();
massOp += blockSize;
// Orthonormalize X against the known eigenvectors and the previous vectors
// Note: Use R as a temporary work space
timeOrtho -= MyWatch.WallTime();
if (nb == bStart) {
if (nFound > 0) {
if (knownEV == 0) {
info = modalTool.massOrthonormalize(Xcurrent, MX, M, Q, nFound, 2, R.Values());
}
else {
Epetra_MultiVector copyQ(View, X, 0, knownEV + localSize);
info = modalTool.massOrthonormalize(Xcurrent, MX, M, copyQ, nFound, 0, R.Values());
}
}
nFound = 0;
}
else {
Epetra_MultiVector copyQ(View, X, 0, knownEV + localSize);
info = modalTool.massOrthonormalize(Xcurrent, MX, M, copyQ, blockSize, 0, R.Values());
}
timeOrtho += MyWatch.WallTime();
// Exit the code when the number of vectors exceeds the space dimension
if (info < 0) {
delete[] work1;
delete[] work2;
if (vectWeight)
delete vectWeight;
return -10;
}
timeStifOp -= MyWatch.WallTime();
K->Apply(Xcurrent, KX);
timeStifOp += MyWatch.WallTime();
stifOp += blockSize;
// Check the orthogonality properties of X
if (verbose > 2) {
if (knownEV + localSize == 0)
accuracyCheck(&Xcurrent, &MX, 0);
else {
Epetra_MultiVector copyQ(View, X, 0, knownEV + localSize);
accuracyCheck(&Xcurrent, &MX, ©Q);
}
if (localVerbose > 0)
cout << endl;
} // if (verbose > 2)
// Define the local stiffness matrix
// Note: S is used as a workspace
timeLocalProj -= MyWatch.WallTime();
for (j = 0; j <= nb; ++j) {
callBLAS.GEMM('T', 'N', blockSize, blockSize, xr,
1.0, X.Values()+(knownEV+j*blockSize)*xr, xr, KX.Values(), xr,
0.0, tmpKK, blockSize);
MyComm.SumAll(tmpKK, S, blockSize*blockSize);
int iC;
for (iC = 0; iC < blockSize; ++iC) {
double *Kpointer = KK + localSize*dimSearch + j*blockSize + iC*dimSearch;
memcpy(Kpointer, S + iC*blockSize, blockSize*sizeof(double));
}
}
timeLocalProj += MyWatch.WallTime();
// Perform a spectral decomposition
timeLocalSolve -= MyWatch.WallTime();
int nevLocal = localSize + blockSize;
info = modalTool.directSolver(localSize+blockSize, KK, dimSearch, 0, 0,
nevLocal, S, dimSearch, theta, localVerbose, 10);
timeLocalSolve += MyWatch.WallTime();
if (info != 0) {
// Stop as spectral decomposition has a critical failure
if (info < 0) {
criticalExit = true;
break;
}
// Restart as spectral decomposition failed
if (localVerbose > 0) {
cout << " Iteration " << outerIter;
cout << "- Failure for spectral decomposition - RESTART with new random search\n";
}
reStart = true;
numRestart += 1;
timeRestart -= MyWatch.WallTime();
Epetra_MultiVector Xinit(View, X, knownEV, blockSize);
Xinit.Random();
timeRestart += MyWatch.WallTime();
nFound = blockSize;
bStart = 0;
break;
} // if (info != 0)
// Update the search space
// Note: Use KX as a workspace
timeLocalUpdate -= MyWatch.WallTime();
callBLAS.GEMM('N', 'N', xr, blockSize, localSize+blockSize, 1.0, X.Values()+knownEV*xr, xr,
S, dimSearch, 0.0, KX.Values(), xr);
timeLocalUpdate += MyWatch.WallTime();
// Apply the mass matrix for the next block
timeMassOp -= MyWatch.WallTime();
if (M)
M->Apply(KX, MX);
timeMassOp += MyWatch.WallTime();
massOp += blockSize;
// Apply the stiffness matrix for the next block
timeStifOp -= MyWatch.WallTime();
K->Apply(KX, R);
timeStifOp += MyWatch.WallTime();
stifOp += blockSize;
// Form the residuals
timeResidual -= MyWatch.WallTime();
if (M) {
for (j = 0; j < blockSize; ++j) {
callBLAS.AXPY(xr, -theta[j], MX.Values() + j*xr, R.Values() + j*xr);
}
}
else {
// Note KX contains the updated block
for (j = 0; j < blockSize; ++j) {
callBLAS.AXPY(xr, -theta[j], KX.Values() + j*xr, R.Values() + j*xr);
}
}
timeResidual += MyWatch.WallTime();
residual += blockSize;
// Compute the norm of residuals
timeNorm -= MyWatch.WallTime();
if (vectWeight) {
R.NormWeighted(*vectWeight, normR);
}
else {
R.Norm2(normR);
}
// Scale the norms of residuals with the eigenvalues
// Count the number of converged eigenvectors
nFound = 0;
for (j = 0; j < blockSize; ++j) {
normR[j] = (theta[j] == 0.0) ? normR[j] : normR[j]/theta[j];
if (normR[j] < tolEigenSolve)
nFound += 1;
} // for (j = 0; j < blockSize; ++j)
timeNorm += MyWatch.WallTime();
// Store the residual history
if (localVerbose > 2) {
memcpy(resHistory + historyCount*blockSize, normR, blockSize*sizeof(double));
spaceSizeHistory[historyCount] = localSize + blockSize;
historyCount += 1;
}
maxSpaceSize = (maxSpaceSize > localSize+blockSize) ? maxSpaceSize : localSize+blockSize;
sumSpaceSize += localSize + blockSize;
// Print information on current iteration
if (localVerbose > 0) {
cout << " Iteration " << outerIter << " - Number of converged eigenvectors ";
cout << knownEV + nFound << endl;
} // if (localVerbose > 0)
if (localVerbose > 1) {
cout << endl;
cout.precision(2);
cout.setf(ios::scientific, ios::floatfield);
for (i=0; i<blockSize; ++i) {
cout << " Iteration " << outerIter << " - Scaled Norm of Residual " << i;
cout << " = " << normR[i] << endl;
}
cout << endl;
cout.precision(2);
for (i=0; i<nevLocal; ++i) {
cout << " Iteration " << outerIter << " - Ritz eigenvalue " << i;
cout.setf((fabs(theta[i]) < 0.01) ? ios::scientific : ios::fixed, ios::floatfield);
cout << " = " << theta[i] << endl;
}
cout << endl;
}
// Exit the loop to treat the converged eigenvectors
if (nFound > 0) {
nb += 1;
offSet = 0;
break;
}
// Apply the preconditioner on the residuals
// Note: Use KX as a workspace
if (maxBlock == 1) {
if (Prec) {
timePrecOp -= MyWatch.WallTime();
Prec->ApplyInverse(R, Xcurrent);
timePrecOp += MyWatch.WallTime();
precOp += blockSize;
}
else {
memcpy(Xcurrent.Values(), R.Values(), blockSize*xr*sizeof(double));
}
timeRestart -= MyWatch.WallTime();
Xcurrent.Update(1.0, KX, -1.0);
timeRestart += MyWatch.WallTime();
break;
} // if (maxBlock == 1)
if (nb == maxBlock - 1) {
nb += 1;
break;
}
Epetra_MultiVector Xnext(View, X, knownEV+localSize+blockSize, blockSize);
if (Prec) {
timePrecOp -= MyWatch.WallTime();
Prec->ApplyInverse(R, Xnext);
timePrecOp += MyWatch.WallTime();
precOp += blockSize;
}
else {
memcpy(Xnext.Values(), R.Values(), blockSize*xr*sizeof(double));
}
} // for (nb = bStart; nb < maxBlock; ++nb)
if (outerIter > maxIterEigenSolve)
break;
if (reStart == true) {
reStart = false;
continue;
}
if (criticalExit == true)
break;
// Store the final converged eigenvectors
if (knownEV + nFound >= numEigen) {
for (j = 0; j < blockSize; ++j) {
if (normR[j] < tolEigenSolve) {
memcpy(X.Values() + knownEV*xr, KX.Values() + j*xr, xr*sizeof(double));
lambda[knownEV] = theta[j];
knownEV += 1;
}
}
if (localVerbose == 1) {
cout << endl;
cout.precision(2);
cout.setf(ios::scientific, ios::floatfield);
for (i=0; i<blockSize; ++i) {
cout << " Iteration " << outerIter << " - Scaled Norm of Residual " << i;
cout << " = " << normR[i] << endl;
}
cout << endl;
}
break;
} // if (knownEV + nFound >= numEigen)
// Treat the particular case of 1 block
if (maxBlock == 1) {
if (nFound > 0) {
double *Xpointer = X.Values() + (knownEV+nFound)*xr;
nFound = 0;
for (j = 0; j < blockSize; ++j) {
if (normR[j] < tolEigenSolve) {
memcpy(X.Values() + knownEV*xr, KX.Values() + j*xr, xr*sizeof(double));
lambda[knownEV] = theta[j];
knownEV += 1;
nFound += 1;
}
else {
memcpy(Xpointer + (j-nFound)*xr, KX.Values() + j*xr, xr*sizeof(double));
}
}
Epetra_MultiVector Xnext(View, X, knownEV + blockSize - nFound, nFound);
Xnext.Random();
}
else {
nFound = blockSize;
}
continue;
}
// Define the restarting block when maxBlock > 1
if (nFound > 0) {
int firstIndex = blockSize;
for (j = 0; j < blockSize; ++j) {
if (normR[j] >= tolEigenSolve) {
firstIndex = j;
break;
}
} // for (j = 0; j < blockSize; ++j)
while (firstIndex < nFound) {
for (j = firstIndex; j < blockSize; ++j) {
if (normR[j] < tolEigenSolve) {
// Swap the j-th and firstIndex-th position
callFortran.SWAP(nb*blockSize, S + j*dimSearch, 1, S + firstIndex*dimSearch, 1);
callFortran.SWAP(1, theta + j, 1, theta + firstIndex, 1);
callFortran.SWAP(1, normR + j, 1, normR + firstIndex, 1);
break;
}
} // for (j = firstIndex; j < blockSize; ++j)
for (j = 0; j < blockSize; ++j) {
if (normR[j] >= tolEigenSolve) {
firstIndex = j;
break;
}
} // for (j = 0; j < blockSize; ++j)
} // while (firstIndex < nFound)
// Copy the converged eigenvalues
memcpy(lambda + knownEV, theta, nFound*sizeof(double));
} // if (nFound > 0)
// Define the restarting size
bStart = ((nb - offSet) > 2) ? (nb - offSet)/2 : 0;
// Define the restarting space and local stiffness
timeRestart -= MyWatch.WallTime();
memset(KK, 0, nb*blockSize*dimSearch*sizeof(double));
for (j = 0; j < bStart*blockSize; ++j) {
KK[j + j*dimSearch] = theta[j + nFound];
}
// Form the restarting space
int oldCol = nb*blockSize;
int newCol = nFound + (bStart+1)*blockSize;
newCol = (newCol > oldCol) ? oldCol : newCol;
callFortran.GEQRF(oldCol, newCol, S, dimSearch, theta, R.Values(), xr*blockSize, &info);
callFortran.ORMQR('R', 'N', xr, oldCol, newCol, S, dimSearch, theta,
X.Values()+knownEV*xr, xr, R.Values(), blockSize*xr, &info);
timeRestart += MyWatch.WallTime();
if (nFound == 0)
offSet += 1;
knownEV += nFound;
maxBlock = (dimSearch/blockSize) - (knownEV/blockSize);
// Put random vectors if the Rayleigh Ritz vectors are not enough
newCol = nFound + (bStart+1)*blockSize;
if (newCol > oldCol) {
Epetra_MultiVector Xnext(View, X, knownEV+blockSize-nFound, nFound);
Xnext.Random();
continue;
}
nFound = 0;
} // while (outerIter <= maxIterEigenSolve)
timeOuterLoop += MyWatch.WallTime();
highMem = (highMem > currentSize()) ? highMem : currentSize();
// Clean memory
delete[] work1;
delete[] work2;
if (vectWeight)
delete vectWeight;
// Sort the eigenpairs
timePostProce -= MyWatch.WallTime();
if ((info == 0) && (knownEV > 0)) {
mySort.sortScalars_Vectors(knownEV, lambda, Q.Values(), Q.MyLength());
}
timePostProce += MyWatch.WallTime();
return (info == 0) ? knownEV : info;
}
void Model :: MD_3D(double le,int BstartID,int beforeN,int newN,double r,double region[3][2])
{
//分子動力学によりnewN個の粒子の位置を最適化 IDがBstartIDからBendIDまでのは境界粒子なので動かさない
double k0=1;
double dt=0.001;
int BendID=beforeN;
//力はax^3+bx^2+dの式を採用。文献[Bubble Mesh Automated Triangular Meshing of Non-Manifold Geometry by Sphere Packing]を参照
double a=(r+1)/(2*r*r-r-1)*k0/(le*le);
double b=-0.5*k0/le-1.5*a*le;
double d=-a*le*le*le-b*le*le;
/////////////
int lastN=beforeN+newN;
//cout<<"F="<<a*le*le*le+b*le*le+d<<" "<<a*1.5*le*1.5*le*1.5*le+b*1.5*le*1.5*le+d<<endl;
vector<double> Fx(newN); //各粒子に働くX方向力
vector<double> Fy(newN); //各粒子に働くY方向力
vector<double> Fz(newN); //各粒子に働くZ方向力
vector<double> Ax(newN,0); //X方向加速度
vector<double> Ay(newN,0); //Y方向加速度
vector<double> Az(newN,0); //Z方向加速度
vector<double> U(newN,0); //X方向速度
vector<double> V(newN,0); //Y方向速度
vector<double> W(newN,0); //Z方向速度
vector<double> visX(newN); //X方向粘性係数
vector<double> visY(newN); //Y方向粘性係数
vector<double> visZ(newN); //Y方向粘性係数
vector<double> KX(newN); //X方向バネ係数
vector<double> KY(newN); //Y方向バネ係数
vector<double> KZ(newN); //Y方向バネ係数
//計算の高速化のために格子を形成 解析幅がr*leで割り切れるとは限らないので、はみ出したところは切り捨て。なので各軸とも正の方向には余裕を持つこと
double grid_width=le*((int)(r+1)); //格子の幅。rを含む整数*le
int grid_sizeX=(int)((region[A_X][1]-region[A_X][0])/grid_width); //X方向の格子の個数
int grid_sizeY=(int)((region[A_Y][1]-region[A_Y][0])/grid_width);
int grid_sizeZ=(int)((region[A_Z][1]-region[A_Z][0])/grid_width);
int grid_SIZE=grid_sizeX*grid_sizeY*grid_sizeZ;
int plane_SIZE=grid_sizeX*grid_sizeY;
int *index=new int[newN]; //各内部粒子を含む格子番号
vector<int> *MESH=new vector<int>[grid_SIZE]; //各メッシュに格納される粒子ID格納
for(int i=BstartID;i<BendID;i++) //まずは境界粒子を格子に格納
{
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int zn=(int)((PART[i].Get_Z()-region[A_Z][0])/grid_width);//Z方向に何個目の格子か
int number=zn*grid_sizeX*grid_sizeY+yn*grid_sizeX+xn;//粒子iを含む格子の番号
MESH[number].push_back(i);
}
for(int k=0;k<newN;k++) //つぎに内部粒子を格納
{
int i=beforeN+k;
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int zn=(int)((PART[i].Get_Z()-region[A_Z][0])/grid_width);//Z方向に何個目の格子か
int number=zn*grid_sizeX*grid_sizeY+yn*grid_sizeX+xn;//粒子iを含む格子の番号
MESH[number].push_back(i);
index[k]=number;
}
//計算開始
for(int t=0;t<100;t++)
{
if(t%10==0 && t>0)
{
//MESHを一度破壊する。
for(int n=0;n<grid_SIZE;n++)
{
size_t size=MESH[n].size();
for(int k=0;k<size;k++) MESH[n].pop_back();
}
for(int i=BstartID;i<BendID;i++) //まずは境界粒子を格子に格納
{
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int zn=(int)((PART[i].Get_Z()-region[A_Z][0])/grid_width);//Z方向に何個目の格子か
int number=zn*grid_sizeX*grid_sizeY+yn*grid_sizeX+xn;//粒子iを含む格子の番号
MESH[number].push_back(i);
}
for(int k=0;k<newN;k++) //つぎに内部粒子を格納
{
int i=beforeN+k;
int xn=(int)((PART[i].Get_X()-region[A_X][0])/grid_width);//X方向に何個目の格子か
int yn=(int)((PART[i].Get_Y()-region[A_Y][0])/grid_width);//Y方向に何個目の格子か
int zn=(int)((PART[i].Get_Z()-region[A_Z][0])/grid_width);//Z方向に何個目の格子か
int number=zn*grid_sizeX*grid_sizeY+yn*grid_sizeX+xn;//粒子iを含む格子の番号
MESH[number].push_back(i);
index[k]=number;
}
}
for(int k=0;k<newN;k++)
{
Fx[k]=0; Fy[k]=0, Fz[k]=0; //初期化
KX[k]=0;KY[k]=0; KZ[k]=0; //バネ係数
}
for(int k=0;k<newN;k++)
{
int i=beforeN+k; //対応する粒子番号
int G_id=index[k]; //格納する格子番号
for(int II=G_id-1;II<=G_id+1;II++)
{
for(int JJ=-1*grid_sizeX;JJ<=grid_sizeX;JJ+=grid_sizeX)
{
for(int KK=-1*plane_SIZE;KK<=plane_SIZE;KK+=plane_SIZE)
{
int M_id=II+JJ+KK;
for(int L=0;L<MESH[M_id].size();L++)
{
int j=MESH[M_id][L];
if(j>=beforeN && j>i) //同じ領域内でかつiより大きな番号なら
{
int J=j-beforeN; //newN内での番号
double x=PART[j].Get_X()-PART[i].Get_X();
double y=PART[j].Get_Y()-PART[i].Get_Y();
double z=PART[j].Get_Z()-PART[i].Get_Z();
double dis=sqrt(x*x+y*y+z*z);
if(dis<r*le) //このloopは自分自身も通過するから、dis!=0は必要
{
double F=a*dis*dis*dis+b*dis*dis+d;
Fx[k]-=F*x/dis; //Fの値が正のときは斥力なので、-=にする
Fy[k]-=F*y/dis;
Fz[k]-=F*z/dis;
Fx[J]+=F*x/dis; //相手粒子の力もここで計算。符号は反転させる
Fy[J]+=F*y/dis;
Fz[J]+=F*z/dis;
double K=3*a*dis*dis+2*b*dis;//バネ係数 力の式の微分に相当
K=sqrt(K*K); //正の値が欲しい。だから負のときに備えて正に変換
KX[k]+=K*x*x/(dis*dis); //kを各方向に分配。ここで、常に正の量が分配されるようにx*x/(dis*dis)となっている
KY[k]+=K*y*y/(dis*dis);
KZ[k]+=K*z*z/(dis*dis);
KX[J]+=K*x*x/(dis*dis); //kを相手粒子にも分配
KY[J]+=K*y*y/(dis*dis);
KZ[J]+=K*z*z/(dis*dis);
}
}
if(j<BendID && j>=BstartID)
{
double x=PART[j].Get_X()-PART[i].Get_X();
double y=PART[j].Get_Y()-PART[i].Get_Y();
double z=PART[j].Get_Z()-PART[i].Get_Z();
double dis=sqrt(x*x+y*y+z*z);
if(dis<r*le && dis>0) //このloopは自分自身は通過しない、dis!=0は不要
{
double F=a*dis*dis*dis+b*dis*dis+d;
Fx[k]-=F*x/dis; //Fの値が正のときは斥力なので、-=にする
Fy[k]-=F*y/dis;
Fz[k]-=F*z/dis;
double K=3*a*dis*dis+2*b*dis;//バネ係数 力の式の微分に相当
K=sqrt(K*K); //正の値が欲しい。だから負のときに備えて正に変換
KX[k]+=K*x*x/(dis*dis); //kを各方向に分配。ここで、常に正の量が分配されるようにx*x/(dis*dis)となっている
KY[k]+=K*y*y/(dis*dis);
KZ[k]+=K*z*z/(dis*dis);
}
}
}
}
}
}
//visX[k]=1.414*sqrt(KX[k]);//このように各軸方向の粘性係数を決める。文献「物理モデルによる自動メッシュ分割」P6参照。ただし質量は1としている。
//visY[k]=1.414*sqrt(KY[k]);
//visZ[k]=1.414*sqrt(KZ[k]);
visX[k]=1.414*sqrt(KX[k]);//このように各軸方向の粘性係数を決める。文献「物理モデルによる自動メッシュ分割」P6参照。ただし質量は1としている。
visY[k]=1.414*sqrt(KY[k]);
visZ[k]=1.414*sqrt(KZ[k]);
Ax[k]=(Fx[k]-visX[k]*U[k]);
Ay[k]=(Fy[k]-visY[k]*V[k]);
Az[k]=(Fz[k]-visZ[k]*W[k]);
}//各粒子の加速度が求まった。
if(t==0) //最初のｽﾃｯﾌﾟ時にdtを決定
{
double MaxAccel=0;
for(int k=0;k<newN;k++)
{
double accel2=Ax[k]*Ax[k]+Ay[k]*Ay[k]+Az[k]*Az[k];
if(accel2>MaxAccel) MaxAccel=accel2;
}
MaxAccel=sqrt(MaxAccel);//最大加速度が求まった
if(MaxAccel!=0)
{
dt=sqrt(0.02*le/MaxAccel);
}
}
for(int k=0;k<newN;k++)//速度と位置の更新
{
int i=beforeN+k;
double u=U[k];
double v=V[k];
double w=W[k];
U[k]+=dt*Ax[k];
V[k]+=dt*Ay[k];
W[k]+=dt*Az[k];
PART[i].Add(dt*(U[k]+u)*0.5, dt*(V[k]+v)*0.5, dt*(W[k]+w)*0.5);
}
//再近接距離がle以下の場合はこれを修正
for(int k=0;k<newN;k++)
{
int i=beforeN+k; //対応する粒子番号
int G_id=index[k]; //格納する格子番号
double mindis=le;
int J=k; //最近接距離の相手粒子
for(int II=G_id-1;II<=G_id+1;II++)
{
for(int JJ=-1*grid_sizeX;JJ<=grid_sizeX;JJ+=grid_sizeX)
{
for(int KK=-1*plane_SIZE;KK<=plane_SIZE;KK+=plane_SIZE)
{
int M_id=II+JJ+KK;
for(int L=0;L<MESH[M_id].size();L++)
{
int j=MESH[M_id][L];
double x=PART[j].Get_X()-PART[i].Get_X();
double y=PART[j].Get_Y()-PART[i].Get_Y();
double z=PART[j].Get_Z()-PART[i].Get_Z();
double dis=sqrt(x*x+y*y+z*z);
if(dis<mindis && i!=j)
{
mindis=dis;
J=j;
}
}
}
}
}
if(J!=i && J<beforeN)//leより近接している相手が境界粒子なら
{
double L=le-mindis;//開くべき距離
double dX=PART[J].Get_X()-PART[i].Get_X();
double dY=PART[J].Get_Y()-PART[i].Get_Y();
double dZ=PART[J].Get_Z()-PART[i].Get_Z();
PART[i].Add(-dX/mindis*L, -dY/mindis*L, -dZ/mindis*L);
}
else if(J!=i && J>=beforeN)//leより近接している相手が内部粒子なら
{
double L=0.5*(le-mindis);//開くべき距離
double dX=PART[J].Get_X()-PART[i].Get_X();
double dY=PART[J].Get_Y()-PART[i].Get_Y();
double dZ=PART[J].Get_Z()-PART[i].Get_Z();
PART[i].Add(-dX/mindis*L, -dY/mindis*L, -dZ/mindis*L);
PART[J].Add(dX/mindis*L, dY/mindis*L, dZ/mindis*L);
}
}//////////*/
}/////MD終了
delete [] index;
delete [] MESH;
}
order_result_t take_water(dispenser_t dispenser){
order_result_t or;
//save the amount of water we already have to check if we managed to "open" a dispenser
uint8_t nb_empty = robot_state.cylinder_nb_empty;
//dispenser positions
int16_t near_pos = 2000-840;
int16_t far_pos = -(1500-620);
//balleater configuration
int16_t balleater_depth = AUTOSET_OFFSET + 35;
int16_t approach_depth = balleater_depth + 60;
int16_t approach_side = 150;
int16_t traj1[4];
int16_t traj2[2];
float angle;
//prepare meca to take water
_wait_meca_ready();
ROME_SENDWAIT_MECA_CMD(&rome_meca,ROME_ENUM_MECA_COMMAND_PREPARE_LOAD_WATER);
_wait_meca_ground_clear();
set_speed(RS_FAST);
switch(dispenser){
case DISPENSER_NEAR:{
ROME_LOG(&rome_paddock,DEBUG,"Going to start area dispenser");
angle = arfast(ROBOT_SIDE_BALLEATER, TABLE_SIDE_MAIN);
//send first position order
or = goto_xya(KX(1200), near_pos + approach_side, angle);
if (or!= ORDER_SUCCESS)
return or;
//prepare next move orders
//dispenser near is alongside Y axis
traj1[0] = KX(1500-approach_depth);
traj1[1] = near_pos + approach_side;
traj1[2] = KX(1500-(balleater_depth+10));
traj1[3] = near_pos;
traj2[0] = KX(1500-300);
traj2[1] = near_pos;
break;
}
case DISPENSER_FAR:{
ROME_LOG(&rome_paddock,INFO,"Going to opposite dispenser");
//send first position order
//go to the bee corner to be near borders for autoset
or = goto_pathfinding_node(PATHFINDING_GRAPH_NODE_OPPOSITE_BEE, arfast(ROBOT_SIDE_BALLEATER, TABLE_SIDE_AUX));
if (or != ORDER_SUCCESS){
ROME_LOG(&rome_paddock,INFO,"Aborting opposite dispenser");
//go back in the middle
goto_pathfinding_node(PATHFINDING_GRAPH_NODE_MIDDLE_BOT, arfast(ROBOT_SIDE_BACK, TABLE_SIDE_DOWN));
return or;
}
//we did a very long move, so try to launch an autoset
//or = goto_xya(KX(-1200), 150, arfast(ROBOT_SIDE_BACK, TABLE_SIDE_DOWN));
//autoset(ROBOT_SIDE_BACK,AUTOSET_DOWN, 0, AUTOSET_OFFSET);
or = goto_xya(KX(-1350), 300, arfast(ROBOT_SIDE_BALLEATER, TABLE_SIDE_AUX));
autoset(ROBOT_SIDE_BALLEATER,AUTOSET_AUX, KX(-1500+AUTOSET_OFFSET), 0);
or = goto_xya(KX(-1200), 300, arfast(ROBOT_SIDE_BALLEATER, TABLE_SIDE_AUX));
//prepare next move orders
//dispenser far is alongside X axis
angle = arfast(ROBOT_SIDE_BALLEATER, TABLE_SIDE_DOWN);
traj1[0] = KX(far_pos-approach_side);
traj1[1] = approach_depth;
traj1[2] = KX(far_pos);
traj1[3] = balleater_depth-25;
traj2[0] = KX(far_pos);
traj2[1] = 500;
break;
}
default:
return ORDER_FAILURE;
}
set_speed(RS_SLOW);
//go to dispenser
or = goto_traj(traj1, angle);
if (or!= ORDER_SUCCESS)
return or;
//take the water
_wait_meca_ready();
ROME_SENDWAIT_MECA_CMD(&rome_meca,ROME_ENUM_MECA_COMMAND_LOAD_WATER);
_wait_meca_ground_clear();
set_speed(RS_NORMAL);
//just go back a little bit
or = goto_traj(traj2, angle);
//if the water in cylinder changed, we scored !
if (nb_empty != robot_state.cylinder_nb_empty)
update_score(10);
set_speed(RS_NORMAL);
return or;
}
float compute_throw_angle(int16_t x, int16_t y){
float target = atan2(KX(1320)-KX(x),2200-y);
float robot = arfast(ROBOT_SIDE_TURBINE,TABLE_SIDE_UP);
return robot - target;
}
