void applyControl(Thread* start, const VectorXd& u, VectorXd* res) { int N = glThreads[startThread]->getThread()->num_pieces(); res->setZero(3*N); Vector3d translation; translation << u(0), u(1), u(2); double dw = 1.0 - u(3)*u(3) - u(4)*u(4) - u(5)*u(5); if (dw < 0) { cout << "Huge differential quaternion: " << endl; } dw = (dw > 0) ? dw : 0.0; dw = sqrt(dw); Eigen::Quaterniond q(dw, u(3),u(4),u(5)); Matrix3d rotation(q); // apply the control u to thread start, and return the new config in res Frame_Motion toMove(translation, rotation); Vector3d end_pos = start->end_pos(); Matrix3d end_rot = start->end_rot(); toMove.applyMotion(end_pos, end_rot); start->set_end_constraint(end_pos, end_rot); start->minimize_energy(); start->toVector(res); }
void planMovement() { // use quaternion interpolation to move closer to end rot Matrix3d end_rot = glThreads[planThread]->getEndRotation(); // figure out angle between quats, spherically interpolate. Matrix3d goal_rot = glThreads[endThread]->getEndRotation(); Eigen::Quaterniond endq(end_rot); Eigen::Quaterniond goalq(goal_rot); Vector3d after_goal = goal_rot*Vector3d::UnitX(); Vector3d after_end = end_rot*Vector3d::UnitX(); double angle = acos(after_goal.dot(after_end)); double t = M_PI/128.0/angle; Eigen::Quaterniond finalq = endq.slerp(t, goalq).normalized(); Matrix3d rotation(finalq*endq.inverse()); // use linear interpolation to move closer to end pos Vector3d end_pos = glThreads[planThread]->getEndPosition(); Vector3d goal_pos = glThreads[endThread]->getEndPosition(); Vector3d translation = goal_pos - end_pos; double step = 2.0; translation.normalize(); translation *= step; // apply the motion Frame_Motion toMove(translation, rotation); toMove.applyMotion(end_pos, end_rot); glThreads[planThread]->set_end_constraint(end_pos, end_rot); glThreads[planThread]->minimize_energy(); glutPostRedisplay(); }
//Console do elevador void console(void) { int goTo, in; //Objetivo, entrada printf ("Console do Elevador.\n"); do { printf (": "); in = getch(); //Recebe tecla goTo = keyToInt(in); //Recebe inteiro if ((goTo >= 0) && (goTo <= 3) && (goTo != position)) //Verifica��o toMove(goTo); //Chama do procedimento (destino) else if (goTo == -1) //Caso a mesma posi��o { toMove(0); //Desce o elevador ate o t�rreo printf ("Sair.\n"); goTo = -1; } else //Qualquer outro valor printf ("Erro.\n"); } while (goTo != -1); //Condi��o do loop }
std::string Player::doMove( const std::string& opponent_move ) { m_move_number++; if( opponent_move != "Start" ) { Move opp_move; toMove( opponent_move, opp_move ); m_ai->opponentMove( opp_move ); } Move move; m_ai->getMove( move ); return toString( move ); }
void ModelGrouper::group( GroupMap & grouped ) { QList<QPersistentModelIndex> persistentGroupIndexes; // If we are already grouped, we need to insert items into existing groups before creating new ones if( mIsGrouped ) { // Get persistent indexes for each group item, because regular ones may be invalidated by // the move call in the loop for( ModelIter it(model()); it.isValid(); ++it ) if( model()->translator(*it) == groupedItemTranslator() ) persistentGroupIndexes.append( *it ); foreach( QPersistentModelIndex idx, persistentGroupIndexes ) { bool isEmptyGroup = model()->rowCount(idx) == 0; QString groupVal = idx.sibling( idx.row(), mGroupColumn ).data( Qt::DisplayRole ).toString(); GroupMap::Iterator mapIt = grouped.find( groupVal ); if( mapIt != grouped.end() ) { QModelIndexList toMove(fromPersist(mapIt.value())); //LOG_5( QString("Moving indexes %1 to existing group item at index %2").arg(indexListToStr(toMove)).arg(indexToStr(idx)) ); model()->move( toMove, idx ); if( isEmptyGroup ) emit groupPopulated( idx ); if( mUpdateScheduled ) { if( !mGroupItemsToUpdate.contains( idx ) ) mGroupItemsToUpdate.append(idx); } else // Tell the group item to update itself based on the added children model()->setData( idx, QVariant(), GroupingUpdate ); grouped.erase( mapIt ); } } // Deal with any now-empty groups for( QList<QPersistentModelIndex>::Iterator it = persistentGroupIndexes.begin(); it != persistentGroupIndexes.end(); ) if( model()->translator(*it) == groupedItemTranslator() && model()->rowCount(*it) == 0 ) { emit groupEmptied(*it); ++it; } else it = persistentGroupIndexes.erase( it ); if( emptyGroupPolicy() == RemoveEmptyGroups ) model()->remove( fromPersist( persistentGroupIndexes ) ); }
void PFManager::computeAttackingUnitActions(BaseAgent* agent, TilePosition goal, bool defensive, bool forceMove) { Unit* unit = agent->getUnit(); if (!defensive || !forceMove) { //if (agent->getUnit()->isStartingAttack()) return; if (agent->getUnit()->isAttacking()) return; if (agent->getUnit()->isSieged()) return; } if (agent->getUnit()->isLoaded()) return; //PF int unitX = unit->getPosition().x(); int unitY = unit->getPosition().y(); float bestP = getAttackingUnitP(agent, unitX, unitY, defensive); float cP = 0; float startP = bestP; int bestX = -1; int bestY = -1; for (int cX = unitX - checkRange; cX <= unitX + checkRange; cX += stepSize) { for (int cY = unitY - checkRange; cY <= unitY + checkRange; cY += stepSize) { if (cX >= 0 && cY >= 0 && cX <= mapW && cY <= mapH) { cP = getAttackingUnitP(agent, cX, cY, defensive); //Broodwar->printf("p(%d,%d)=%d",cX,cY,cP); if (cP != bestP) { bestP = cP; bestX = cX; bestY = cY; } } } } if (bestX >= 0 && bestY >= 0) { Position toMove(bestX, bestY); if (!defensive) { unit->attack(toMove); } else { unit->rightClick(toMove); } return; } //EndPF TilePosition checkpoint = goal; if (agent->getSquadID() >= 0) { Squad* sq = Commander::getInstance()->getSquad(agent->getSquadID()); if (sq != NULL) { checkpoint = sq->nextMovePosition(); } } if (goal.x() != -1) { moveToGoal(agent, checkpoint, goal); } }