cfixmat operator*(const cfixmat &a, const imat &b)
{
it_assert_debug(a.cols() == b.rows(), "operator*: wrong sizes");
cfixmat r(a.rows(), b.cols());
CFix tmp;
int i, j, k;
CFix *tr = r._data();
const CFix *t1;
const int *t2 = b._data();
for (i = 0; i < r.cols(); i++) {
for (j = 0; j < r.rows(); j++) {
tmp = CFix(0);
t1 = a._data() + j;
for (k = a.cols(); k > 0; k--) {
tmp += *(t1) * *(t2++);
t1 += a.rows();
}
*(tr++) = tmp;
t2 -= b.rows();
}
t2 += b.rows();
}
return r;
}
cmat operator+(const imat &a, const cmat &b)
{
it_assert_debug(a.cols() == b.cols() && a.rows() == b.rows(), "operator+(): sizes does not match");
cmat temp(b);
for (int i = 0;i < a.rows();i++) {
for (int j = 0;j < a.cols();j++) {
temp(i, j) += std::complex<double>(static_cast<double>(a(i, j)), 0.0);
}
}
return temp;
}
mat operator+(const imat &a, const mat &b)
{
it_assert_debug(a.cols() == b.cols() && a.rows() == b.rows(), "operator+(): sizes does not match");
mat temp(b);
for (int i = 0;i < a.rows();i++) {
for (int j = 0;j < a.cols();j++) {
temp(i, j) += (double)a(i, j);
}
}
return temp;
}
cfixmat operator+(const cfixmat &a, const imat &b)
{
it_assert_debug(a.cols() == b.cols() && a.rows() == b.rows(), "operator+(): sizes do not match");
cfixmat temp(a);
for (int i = 0; i < a.rows(); i++) {
for (int j = 0; j < a.cols(); j++) {
temp(i, j) += b(i, j);
}
}
return temp;
}
imat Unit::recursive_find_p_vectors (const imat &per)
{
arma::imat result(per.n_rows,pow(3,per.n_cols)-1);
result.col(0) = per.col(0);
result.col(1) = -per.col(0);
if(per.n_cols != 1)
{
imat tmp1 = recursive_find_p_vectors(per.cols(1,per.n_cols-1));
imat tmp2 = tmp1;
result.cols(2,1+tmp1.n_cols) = tmp1;
tmp1.each_col() += per.col(0);
result.cols(2+tmp1.n_cols,1+tmp1.n_cols*2) = tmp1;
tmp2.each_col() -= per.col(0);
result.cols(2+tmp1.n_cols*2,1+tmp1.n_cols*3) = tmp2;
}
return result;
};
/** The goal of this function is to initialize the AStar algorithm,
* including any data structures which you are to use in the
* computation of the next state
* @param map This is the map which you are given
* @param goal This is the goal of the robot
*/
AStar::AStar(imat map, ivec &goal, AStarProp prop) :
isComplete(false),
isImpossible(false),
goal(goal),
prop(prop) {
this->map = map.t();
assert(0 <= goal(0) && goal(0) < (int)this->map.n_rows &&
0 <= goal(1) && goal(1) < (int)this->map.n_cols);
}
void electrodeDeterminer(mat pos, imat& xp1Electrodes, imat& xn1Electrodes, imat& yp1Electrodes, imat& yn1Electrodes, imat& xp2Electrodes, imat& xn2Electrodes, imat& yp2Electrodes, imat& yn2Electrodes, int& numberOfElectrodes, double& xpElecWallMod, double& xnElecWallMod, double& ypElecWallMod, double& ynElecWallMod) {
//find our cutoffs for what is and and isn't an electrode.
double xp1CutOff;
double xn1CutOff;
double xp2CutOff;
double xn2CutOff;
if (numberOfElectrodes >= 2) {
mat xValues = pos.col(0);
xp1CutOff = xValues.max() + xpElecWallMod;
xp2CutOff = xValues.max() + xpElecWallMod * 2;
xn1CutOff = xValues.min() + xnElecWallMod;
xn2CutOff = xValues.min() + xnElecWallMod * 2;
}
double yp1CutOff;
double yn1CutOff;
double yp2CutOff;
double yn2CutOff;
if (numberOfElectrodes == 4) {
mat yValues = pos.col(1);
yp1CutOff = yValues.max() + ypElecWallMod;
yp2CutOff = yValues.max() + ypElecWallMod * 2;
yn1CutOff = yValues.min() + ynElecWallMod;
yn2CutOff = yValues.min() + ynElecWallMod * 2;
}
//cout << "CUTOFFS XP XN YP YN " << xpCutOff << " " << xnCutOff << " " << ypCutOff << " " << ynCutOff << endl;
///system("read -p \"Number of electrodes changed! Push enter to unpause.\" key");
//initialize matrixes with junk values.
imat newxp1;
newxp1 << -1 << endr;
imat newxn1;
newxn1 << -1 << endr;
imat newyp1;
newyp1 << -1 << endr;
imat newyn1;
newyn1 << -1 << endr;
imat newxp2;
newxp2 << -1 << endr;
imat newxn2;
newxn2 << -1 << endr;
imat newyp2;
newyp2 << -1 << endr;
imat newyn2;
newyn2 << -1 << endr;
// loop over atoms to see if they can be electrodes.
for (int i = 0; i < pos.n_rows; i++) {
if (numberOfElectrodes >= 2) {
if (pos(i, 0) > xp1CutOff) {
imat toBeAdded;
toBeAdded << i << endr;
newxp1 = join_cols(newxp1, toBeAdded);
}
if (pos(i, 0) > xp2CutOff && pos(i, 0) < xp1CutOff) {
imat toBeAdded;
toBeAdded << i << endr;
newxp2 = join_cols(newxp2, toBeAdded);
}
if (pos(i, 0) < xn1CutOff) {
imat toBeAdded;
toBeAdded << i << endr;
newxn1 = join_cols(newxn1, toBeAdded);
}
if (pos(i, 0) < xn2CutOff && pos(i, 0) > xn1CutOff) {
imat toBeAdded;
toBeAdded << i << endr;
newxn2 = join_cols(newxn2, toBeAdded);
}
}
if (numberOfElectrodes == 4) {
if (pos(i, 1) > yp1CutOff) {
imat toBeAdded;
toBeAdded << i << endr;
newyp1 = join_cols(newyp1, toBeAdded);
//cout << "YP1";
}
if (pos(i, 1) > yp2CutOff && pos(i, 1) < yp1CutOff) {
imat toBeAdded;
toBeAdded << i << endr;
newyp2 = join_cols(newyp2, toBeAdded);
// cout << "YP2";
}
if (pos(i, 1) < yn1CutOff) {
imat toBeAdded;
toBeAdded << i << endr;
newyn1 = join_cols(newyn1, toBeAdded);
// cout << "YN1";
}
if (pos(i, 1) < yn2CutOff && pos(i, 1) > yn1CutOff) {
imat toBeAdded;
toBeAdded << i << endr;
newyn2 = join_cols(newyn2, toBeAdded);
// cout << "YN2";
}
}
}
//shave junk values.
newxp1.shed_row(0);
newxn1.shed_row(0);
newyp1.shed_row(0);
newyn1.shed_row(0);
newxp2.shed_row(0);
newxn2.shed_row(0);
newyp2.shed_row(0);
newyn2.shed_row(0);
//We need to compare our newly derived electrode atoms with the old ones;
//they should not have changed.
//check if this is the first run, i.e. the arrays are null
if (xp1Electrodes.is_empty()) {
//simple initilization.
if (numberOfElectrodes >= 2) {
xp1Electrodes = newxp1;
xn1Electrodes = newxn1;
xp2Electrodes = newxp2;
xn2Electrodes = newxn2;
}
if (numberOfElectrodes == 4) {
yp1Electrodes = newyp1;
yn1Electrodes = newyn1;
yp2Electrodes = newyp2;
yn2Electrodes = newyn2;
}
} else {
//check that there are the same number of new and the old electrode atoms
if (numberOfElectrodes >= 2) {
if ((newxp1.size() != xp1Electrodes.size()) || (newxn1.size() != xn1Electrodes.size())
) {
//cout << "NUMBER OF X ELECTRODES CHANGED!!!!!!!!!!!!!!!!!! " << endl;
system("read -p \"Number x1 of electrodes changed! Push enter to unpause.\" key");
}
if ((newxp2.size() != xp2Electrodes.size()) || (newxn2.size() != xn2Electrodes.size())
) {
//cout << "NUMBER OF X ELECTRODES CHANGED!!!!!!!!!!!!!!!!!! " << endl;
system("read -p \"Number x2 of electrodes changed! Push enter to unpause.\" key");
}
}
if (numberOfElectrodes == 4) {
if ((newyp1.size() != yp1Electrodes.size()) || (newyn1.size() != yn1Electrodes.size())) {
//cout << "NUMBER OF Y ELECTRODES CHANGED!!!!!!!!!!!!!!!!!! " << endl;
system("read -p \"Number of y1 electrodes changed! Push enter to unpause.\" key");
}
if ((newyp2.size() != yp2Electrodes.size()) || (newyn2.size() != yn2Electrodes.size())) {
//cout << "NUMBER OF Y ELECTRODES CHANGED!!!!!!!!!!!!!!!!!! " << endl;
system("read -p \"Number of y2 electrodes changed! Push enter to unpause.\" key");
}
}
//number of electrodes the same, fairly ok to assume they did not change.
if (numberOfElectrodes >= 2) {
xp1Electrodes = newxp1;
xp2Electrodes = newxp2;
xn1Electrodes = newxn1;
xn2Electrodes = newxn2;
}
if (numberOfElectrodes == 4) {
yp1Electrodes = newyp1;
yp2Electrodes = newyp2;
yn1Electrodes = newyn1;
yn2Electrodes = newyn2;
}
}
}