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; }