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