void Rubik::rotateCubeU() {
U(false);
Di(false);
this->shift(this->middle, 4, 5, 3, 0);
this->shift(this->edges, 1, 10, 11, 15);
this->shift(this->edges, 23, 3, 13, 22);
std::map<std::string, std::string> tempMap(movesDictionary);
//Dictionary
movesDictionary[_R] = tempMap[_B];
movesDictionary[_Ri] = tempMap[_Bi];
movesDictionary[_R2] = tempMap[_B2];
movesDictionary[_L] = tempMap[_F];
movesDictionary[_Li] = tempMap[_Fi];
movesDictionary[_L2] = tempMap[_F2];
/*movesDictionary[_U] = tempMap[_U];
movesDictionary[_Ui] = tempMap[_Ui];
movesDictionary[_U2] = tempMap[_U2];
movesDictionary[_D] = tempMap[_D];
movesDictionary[_Di] = tempMap[_Di];
movesDictionary[_D2] = tempMap[_D2];*/
movesDictionary[_B] = tempMap[_L];
movesDictionary[_Bi] = tempMap[_Li];
movesDictionary[_B2] = tempMap[_L2];
movesDictionary[_F] = tempMap[_R];
movesDictionary[_Fi] = tempMap[_Ri];
movesDictionary[_F2] = tempMap[_R2];
}
void Cube::RotateLeft()
{
U();
SaveState();
subCubes[0][1][0] = ROTATE_LEFT(oldSubCubes[0][1][2]);
subCubes[1][1][0] = ROTATE_LEFT(oldSubCubes[0][1][1]);
subCubes[2][1][0] = ROTATE_LEFT(oldSubCubes[0][1][0]);
subCubes[0][1][1] = ROTATE_LEFT(oldSubCubes[1][1][2]);
subCubes[1][1][1] = ROTATE_LEFT(oldSubCubes[1][1][1]);
subCubes[2][1][1] = ROTATE_LEFT(oldSubCubes[1][1][0]);
subCubes[0][1][2] = ROTATE_LEFT(oldSubCubes[2][1][2]);
subCubes[1][1][2] = ROTATE_LEFT(oldSubCubes[2][1][1]);
subCubes[2][1][2] = ROTATE_LEFT(oldSubCubes[2][1][0]);
Di();
}
INode* FindNodeRef(ReferenceTarget *Rt)
{
DependentIterator Di(Rt);
ReferenceMaker *Rm;
INode *Nd = NULL;
while (Rm = Di.Next())
{
if(Rm->SuperClassID() == BASENODE_CLASS_ID)
{
return (INode *)Rm;
}
Nd = FindNodeRef((ReferenceTarget *)Rm);
if(Nd)
{
return(Nd);
}
}
return(NULL);
}
String Controller::call(Topic &topic) {
// MQTT state information via API-call
if (topic.modifyTopic(0) == "event/mqtt/connected") {
if (topic.argIs(0, "1")) {
on_mqtt_connected();
} else {
on_mqtt_disconnected();
}
}
// D("Controller: begin call");
// set
if (topic.itemIs(1, "set")) {
if (topic.itemIs(2, "ffs")) {
return ffs.set(topic);
} else if (topic.itemIs(2, "clock")) {
return clock.set(topic);
} else if (topic.itemIs(2, "esp")) {
return espTools.set(topic);
} else if (topic.itemIs(2, "wifi")) {
return wifi.set(topic);
} else if (topic.itemIs(2, "device")) {
if (topic.itemIs(3, "fillDashboard")) {
return device.fillDashboard();
} else {
return device.set(topic);
}
} else if (topic.itemIs(2, "controller")) {
if (topic.itemIs(3, "reconnectDelay")) {
D("set reconnectDelay");
Di("arg=", topic.getArgAsLong(0));
reconnectDelay = topic.getArgAsLong(0);
if (reconnectDelay < 1)
reconnectDelay = 1;
if (reconnectDelay > RECONNECT_DELAY_MAX)
reconnectDelay = RECONNECT_DELAY_MAX;
reconnectDelayed = 0;
return TOPIC_OK;
} else {
return TOPIC_NO;
}
} else {
return TOPIC_NO;
}
// get
} else if (topic.itemIs(1, "get")) {
if (topic.itemIs(2, "ffs")) {
return ffs.get(topic);
} else if (topic.itemIs(2, "clock")) {
return clock.get(topic);
} else if (topic.itemIs(2, "esp")) {
return espTools.get(topic);
} else if (topic.itemIs(2, "wifi")) {
return wifi.get(topic);
} else if (topic.itemIs(2, "device")) {
if(topic.itemIs(3, "flags")) {
return fwConfig();
} else if(topic.itemIs(3, "version")) {
return device.getVersion();
} else if (topic.itemIs(3, "type")) {
return device.getType();
} else if (topic.itemIs(3, "dashboard")) {
return device.getDashboard();
} else {
return device.get(topic);
}
} else {
return TOPIC_NO;
}
} else {
return TOPIC_NO;
}
// D("Controller: end call");
}
void Cube::DoMethod(CubeRotateMethod method)
{
switch (method)
{
case ROTATE_NONE:
case ROTATE_NONEi:
break;
case ROTATE_FRONT:
F();
break;
case ROTATE_BACK:
B();
break;
case ROTATE_LEFT:
L();
break;
case ROTATE_RIGHT:
R();
break;
case ROTATE_UP:
U();
break;
case ROTATE_DOWN:
D();
break;
case ROTATE_FRONTi:
Fi();
break;
case ROTATE_BACKi:
Bi();
break;
case ROTATE_LEFTi:
Li();
break;
case ROTATE_RIGHTi:
Ri();
break;
case ROTATE_UPi:
Ui();
break;
case ROTATE_DOWNi:
Di();
break;
case ROTATE_WHOLEX:
RotateUp();
break;
case ROTATE_WHOLEY:
RotateLeft();
break;
case ROTATE_WHOLEZ:
RotateClockwise();
break;
case ROTATE_WHOLEXi:
RotateDown();
break;
case ROTATE_WHOLEYi:
RotateRight();
break;
case ROTATE_WHOLEZi:
RotateCounterClockwise();
break;
default:
break;
}
}
void ADMMCut::CalculateQ(const VX _D, SpMat &Q)
{
// Construct Hessian Matrix for D-Qp problem
Q.resize(6 * Ns_, Nd_);
vector<Eigen::Triplet<double>> Q_list;
for (int i = 0; i < Ns_; i++)
{
int u = ptr_dualgraph_->v_orig_id(i);
WF_edge *edge = ptr_frame_->GetNeighborEdge(u);
while (edge != NULL)
{
int e_id = ptr_dualgraph_->e_dual_id(edge->ID());
if (e_id != -1)
{
int v = edge->pvert_->ID();
int j = ptr_dualgraph_->v_dual_id(v);
MX eKuu = ptr_stiff_->eKv(edge->ID());
MX eKeu = ptr_stiff_->eKe(edge->ID());
VX Fe = ptr_stiff_->Fe(edge->ID());
VX Di(6);
VX Dj(6);
if (i < Ns_ && j < Ns_)
{
for (int k = 0; k < 6; k++)
{
Di[k] = _D[6 * i + k];
Dj[k] = _D[6 * j + k];
}
}
else
{
if (i < Ns_)
{
for (int k = 0; k < 6; k++)
{
Di[k] = _D[6 * i + k];
Dj[k] = 0;
}
}
if (j < Ns_)
{
for (int k = 0; k < 6; k++)
{
Di[k] = 0;
Dj[k] = _D[6 * j + k];
}
}
}
VX Gamma = eKuu * Di + eKeu * Dj - Fe;
for (int k = 0; k < 6; k++)
{
Q_list.push_back(Triplet<double>(6 * i + k, e_id, Gamma[k]));
}
}
edge = edge->pnext_;
}
}
Q.setFromTriplets(Q_list.begin(), Q_list.end());
}
int prepare(const size_t Ntracks, gsl_vector * v_alpha, gsl_vector * v_d, gsl_matrix * m_D) {
size_t i;
gsl_matrix * ma_D[Ntracks];
gsl_matrix * ma_E[Ntracks];
gsl_vector * va_d[Ntracks];
gsl_vector_view v_alpha_vertex_view = gsl_vector_subvector(v_alpha,0,6);
gsl_vector * v_vertex = &v_alpha_vertex_view.vector;
for(i = 0; i < Ntracks; i++) {
gsl_vector_view v_alpha_track_view = gsl_vector_subvector(v_alpha,3+i*6,6);
gsl_vector * v_track = &v_alpha_track_view.vector;
ma_D[i] = gsl_matrix_calloc(2,6);
ma_E[i] = gsl_matrix_calloc(2,3);
va_d[i] = gsl_vector_calloc(2);
Di(v_vertex,v_track,ma_D[i]);
Ei(v_vertex,v_track,ma_E[i]);
di(v_vertex,v_track,va_d[i]);
}
/* Build generalised D matrix */
gsl_matrix * m_E = gsl_matrix_calloc(2*Ntracks,3);
stack_matrix_array(ma_E,Ntracks,m_E);
gsl_matrix * m_zero = gsl_matrix_calloc(2,6); /* Temporary matrix, used for padding */
/* First build cols */
gsl_matrix * ma_Dcols[Ntracks+1]; /* +1 First col is for E */
ma_Dcols[0] = m_E;
size_t j,k;
gsl_matrix * ma_Dpieces[Ntracks];
/* Allocate the memory */
for(i = 0; i < Ntracks; i++) {
ma_Dcols[i+1] = gsl_matrix_calloc(2*Ntracks,6); /* +1 First col is for E */
}
for(i = 0; i < Ntracks; i++) {
/* Compute blocs order */
for(k = 0; k < Ntracks; k++) {
if(i == k) ma_Dpieces[k] = ma_D[k];
else ma_Dpieces[k] = m_zero;
}
/* Build a column */
stack_matrix_array(ma_Dpieces,Ntracks,ma_Dcols[i+1]); /* +1 First col is for E */
}
/* Juxtapose the cols. */
juxtapose_matrix_array(ma_Dcols,Ntracks+1,m_D); /* +1 First col is for E */
/* Build generalised d matrix */
stack_vector_array(va_d,Ntracks,v_d);
/* Clean the mess */
for(i = 0; i < Ntracks; i++) {
gsl_matrix_free(ma_D[i]);
gsl_matrix_free(ma_E[i]);
gsl_vector_free(va_d[i]);
gsl_matrix_free(ma_Dcols[i+1]); /* +1 First col is for E (bloc pointed by [0] will be freed later) */
}
gsl_matrix_free(m_E);
gsl_matrix_free(m_zero);
return 0;
}
