double footstepHeuristicStraight(
SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph)
{
FootstepState::Ptr state = node->getState();
FootstepState::Ptr goal = graph->getGoal(state->getLeg());
Eigen::Vector3f state_pos(state->getPose().translation());
Eigen::Vector3f goal_pos(goal->getPose().translation());
return (std::abs((state_pos - goal_pos).norm() / graph->maxSuccessorDistance()));
}
double footstepHeuristicFollowPathLine(
SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph)
{
FootstepState::Ptr state = node->getState();
FootstepState::Ptr goal = graph->getGoal(state->getLeg());
Eigen::Vector3f goal_pos(goal->getPose().translation());
Eigen::Vector3f state_pos(state->getPose().translation());
Eigen::Vector3f state_mid_pos;
if (state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
Eigen::Vector3f p(0, -0.1, 0);
state_mid_pos = state->getPose() * p;
} else { // Right
Eigen::Vector3f p(0, 0.1, 0);
state_mid_pos = state->getPose() * p;
}
double dist, to_goal, alp;
int idx;
Eigen::Vector3f foot;
dist = graph->heuristic_path_->distanceWithInfo(state_mid_pos, foot, to_goal, idx, alp);
//jsk_recognition_utils::FiniteLine::Ptr ln = graph->heuristic_path_->at(idx);
Eigen::Vector3f dir = graph->heuristic_path_->getDirection(idx);
Eigen::Quaternionf path_foot_rot;
path_foot_rot.setFromTwoVectors(state->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX(),
dir);
double path_foot_theta = acos(path_foot_rot.w()) * 2;
if (path_foot_theta > M_PI) {
path_foot_theta = 2.0 * M_PI - path_foot_theta;
// foot_theta : [0, M_PI]
}
double step_cost = to_goal / graph->maxSuccessorDistance();
double follow_cost = dist / 0.02; // ???
double path_foot_rot_cost = path_foot_theta / graph->maxSuccessorRotation();
Eigen::Vector3f goal_diff = goal_pos - state_pos;
Eigen::Quaternionf goal_foot_rot;
goal_foot_rot.setFromTwoVectors(state->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX(),
goal->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX());
double goal_foot_theta = acos(goal_foot_rot.w()) * 2;
if (goal_foot_theta > M_PI) {
goal_foot_theta = 2.0 * M_PI - goal_foot_theta;
}
double goal_foot_rot_cost = goal_foot_theta / graph->maxSuccessorRotation();
//return step_cost + follow_cost + (4.0 * goal_foot_rot_cost) + (0.5 * path_foot_rot_cost);
return 2*(step_cost + follow_cost + (0.5 * path_foot_rot_cost));
}
double footstepHeuristicStepCost(
SolverNode<FootstepState, FootstepGraph>::Ptr node, FootstepGraph::Ptr graph,
double first_rotation_weight,
double second_rotation_weight)
{
FootstepState::Ptr state = node->getState();
FootstepState::Ptr goal = graph->getGoal(state->getLeg());
Eigen::Vector3f goal_pos(goal->getPose().translation());
Eigen::Vector3f diff_pos(goal_pos - state->getPose().translation());
diff_pos[2] = 0.0; // Ignore z distance
Eigen::Quaternionf first_rot;
// Eigen::Affine3f::rotation is too slow because it calls SVD decomposition
first_rot.setFromTwoVectors(state->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX(),
diff_pos.normalized());
Eigen::Quaternionf second_rot;
second_rot.setFromTwoVectors(diff_pos.normalized(),
goal->getPose().matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX());
// is it correct??
double first_theta = acos(first_rot.w()) * 2;
double second_theta = acos(second_rot.w()) * 2;
if (isnan(first_theta)) {
first_theta = 0;
}
if (isnan(second_theta)) {
second_theta = 0;
}
// acos := [0, M_PI]
if (first_theta > M_PI) {
first_theta = 2.0 * M_PI - first_theta;
}
if (second_theta > M_PI) {
second_theta = 2.0 * M_PI - second_theta;
}
//return (Eigen::Vector2f(diff_pos[0], diff_pos[1]).norm() / graph->maxSuccessorDistance()) + std::abs(diff_pos[2]) / 0.2 +
return (diff_pos.norm() / graph->maxSuccessorDistance()) + std::abs(diff_pos[2]) / 0.2 +
(first_theta * first_rotation_weight + second_theta * second_rotation_weight) / graph->maxSuccessorRotation();
}
void noShip::HandleEvent(const unsigned int id)
{
current_ev = 0;
switch(id)
{
// Laufevent
case 0:
{
// neue Position einnehmen
Walk();
// entscheiden, was als nächstes zu tun ist
Driven();
} break;
default:
{
switch(state)
{
default: break;
case STATE_EXPEDITION_LOADING:
{
// Schiff ist nun bereit und Expedition kann beginnen
state = STATE_EXPEDITION_WAITING;
// Spieler benachrichtigen
if(GAMECLIENT.GetPlayerID() == this->player)
GAMECLIENT.SendPostMessage(new ShipPostMsg(_("A ship is ready for an expedition."), PMC_GENERAL, GAMECLIENT.GetPlayer(player).nation, pos));
// KI Event senden
GAMECLIENT.SendAIEvent(new AIEvent::Location(AIEvent::ExpeditionWaiting, pos), player);
} break;
case STATE_EXPLORATIONEXPEDITION_LOADING:
{
// Schiff ist nun bereit und Expedition kann beginnen
ContinueExplorationExpedition();
} break;
case STATE_EXPLORATIONEXPEDITION_WAITING:
{
// Schiff ist nun bereit und Expedition kann beginnen
ContinueExplorationExpedition();
} break;
case STATE_EXPEDITION_UNLOADING:
{
// Hafen herausfinden
MapPoint goal_pos(gwg->GetHarborPoint(goal_harbor_id));
noBase* hb = gwg->GetNO(goal_pos);
if(hb->GetGOT() == GOT_NOB_HARBORBUILDING)
{
Goods goods;
memset(&goods, 0, sizeof(Goods));
unsigned char nation = players->getElement(player)->nation;
goods.goods[GD_BOARDS] = BUILDING_COSTS[nation][BLD_HARBORBUILDING].boards;
goods.goods[GD_STONES] = BUILDING_COSTS[nation][BLD_HARBORBUILDING].stones;
goods.people[JOB_BUILDER] = 1;
static_cast<nobBaseWarehouse*>(hb)->AddGoods(goods);
// Wieder idlen und ggf. neuen Job suchen
StartIdling();
players->getElement(player)->GetJobForShip(this);
}
else
{
// target harbor for unloading doesnt exist anymore -> set state to driving and handle the new state
state = STATE_EXPEDITION_DRIVING;
HandleState_ExpeditionDriving();
}
} break;
case STATE_EXPLORATIONEXPEDITION_UNLOADING:
{
// Hafen herausfinden
MapPoint goal_pos(gwg->GetHarborPoint(goal_harbor_id));
noBase* hb = gwg->GetNO(goal_pos);
unsigned old_visual_range = GetVisualRange();
if(hb->GetGOT() == GOT_NOB_HARBORBUILDING)
{
// Späher wieder entladen
Goods goods;
memset(&goods, 0, sizeof(Goods));
goods.people[JOB_SCOUT] = SCOUTS_EXPLORATION_EXPEDITION;
static_cast<nobBaseWarehouse*>(hb)->AddGoods(goods);
// Wieder idlen und ggf. neuen Job suchen
StartIdling();
players->getElement(player)->GetJobForShip(this);
}
else
{
// target harbor for unloading doesnt exist anymore -> set state to driving and handle the new state
state = STATE_EXPLORATIONEXPEDITION_DRIVING;
HandleState_ExplorationExpeditionDriving();
}
// Sichtbarkeiten neu berechnen
gwg->RecalcVisibilitiesAroundPoint(pos, old_visual_range, player, NULL);
} break;
case STATE_TRANSPORT_LOADING:
{
StartTransport();
} break;
case STATE_TRANSPORT_UNLOADING:
{
// Hafen herausfinden
remaining_sea_attackers = 0; //can be 1 in case we had sea attackers on board - set to 1 for a special check when the return harbor is destroyed to set the returning attackers goal to 0
MapPoint goal_pos(gwg->GetHarborPoint(goal_harbor_id));
noBase* hb = gwg->GetNO(goal_pos);
if(hb->GetGOT() == GOT_NOB_HARBORBUILDING)
{
static_cast<nobHarborBuilding*>(hb)->ReceiveGoodsFromShip(figures, wares);
figures.clear();
wares.clear();
// Hafen bescheid sagen, dass er das Schiff nun nutzen kann
static_cast<nobHarborBuilding*>(hb)->ShipArrived(this);
// Hafen hat keinen Job für uns?
if (state == STATE_TRANSPORT_UNLOADING)
{
// Wieder idlen und ggf. neuen Job suchen
StartIdling();
players->getElement(player)->GetJobForShip(this);
}
}
else
{
// target harbor for unloading doesnt exist anymore -> set state to driving and handle the new state
state = STATE_TRANSPORT_DRIVING;
HandleState_TransportDriving();
}
} break;
case STATE_SEAATTACK_LOADING:
{
StartSeaAttack();
} break;
case STATE_SEAATTACK_WAITING:
{
// Nächsten Soldaten nach draußen beordern
if(figures.empty())
break;
nofAttacker* attacker = static_cast<nofAttacker*>(*figures.begin());
// Evtl. ist ein Angreifer schon fertig und wieder an Board gegangen
// der darf dann natürlich nicht noch einmal raus, sonst kann die schöne Reise
// böse enden
if(attacker->IsSeaAttackCompleted())
break;
figures.pop_front();
gwg->AddFigure(attacker, pos);
current_ev = em->AddEvent(this, 30, 1);
attacker->StartAttackOnOtherIsland(pos, GetObjId());
;
};
case STATE_SEAATTACK_UNLOADING:
{
} break;
}
} break;
}
}
bool EnhancedIKSolver::solve_chain_keep_next_bone_pos(unsigned int root_bone,
unsigned int end_bone, const float3& goal_position, int current_frame,
bool force_position) {
//If the end bone does not have child call the simpler method
//this one is for keep the chain after end_bone in the same state
if (skeleton->get_node(end_bone)->get_num_children() == 0) {
return solve_chain(root_bone, end_bone, goal_position, current_frame);
}
std::vector<int> indices;
fill_chain(root_bone, end_bone, current_frame, indices);
Node* next_bone = skeleton->get_node(end_bone)->children[0].get();
osg::Vec3 next_bone_pos = next_bone->get_end_bone_global_pos(current_frame);
osg::Quat next_bone_child_glob_prev_rot;
if (next_bone->get_num_children() != 0) {
next_bone_child_glob_prev_rot = next_bone->children[0]->get_global_rot(
current_frame);
}
osg::Vec3 goal_pos(goal_position.x, goal_position.y, goal_position.z);
//Calculate vector from next bone to goal position
osg::Vec3 dir_vec = goal_pos - next_bone_pos;
dir_vec.normalize();
float next_bone_length = next_bone->get_length();
//Closest point in the sphere of movement is
//centre of the sphere (next_bone_pos) plus its radius ( next_bone_length )
//looking at the direction of the previous goal
goal_pos = next_bone_pos + dir_vec * next_bone_length;
//Calculate in root_bone coordinate system where is the goal position
osg::Matrix m;
skeleton->get_node(root_bone)->get_node_world_matrix_origin(current_frame,
m);
osg::Vec3 goal_position_start_bone = goal_pos * m;
//Solve and update the rotations
bool solve_succes = ik_solver.solve_chain(
make_float3(goal_position_start_bone._v));
if (solve_succes || force_position) {
int j = 0;
for (unsigned int i = 0; i < indices.size(); i++) {
Node * node = skeleton->get_node(indices[i]);
float4 new_rot;
ik_solver.get_rotation_joint(j, new_rot);
node->quat_arr.at(current_frame).set(new_rot.x, new_rot.y,
new_rot.z, new_rot.w);
j++;
}
//Put next bone back to where it was
float3 next_bone_pos_f3 = make_float3(next_bone_pos._v);
//Prev rotations will be overwritten by solve_chain so
//make a copy in case of failure
std::vector<osg::Quat> copy_prev_rots(previous_rotations);
if (!solve_chain(end_bone + 1, end_bone + 1, next_bone_pos_f3,
current_frame)) {
previous_rotations = copy_prev_rots;
undo_rotations(indices, current_frame);
return false;
}
//Correct next bone child rotation
//If child previous global rotation was
//R = R3 * R2 * R1 * R0
//Where R3 is child, R2 parent, etc
//New state is
//R = R3' * R2' * R1' * R0'
//Solving for new R3' that maintains previous global rotation
// R3' = R * inv(R2' * R1' * R0')
if (next_bone->get_num_children() != 0) {
next_bone->children[0]->quat_arr.at(current_frame) =
next_bone_child_glob_prev_rot
* next_bone->get_global_rot(current_frame).inverse();
}
return true;
} else {
return false;
}
}
