/** Finds the starting position which is closest to the kart.
* \param kart The kart for which a rescue position needs to be determined.
*/
unsigned int OverWorld::getRescuePositionIndex(AbstractKart *kart)
{
// find closest point to drop kart on
const int start_spots_amount = getNumberOfRescuePositions();
assert(start_spots_amount > 0);
int closest_id = -1;
float closest_distance = 999999999.0f;
for (int n=0; n<start_spots_amount; n++)
{
const btTransform &s = getStartTransform(n);
const Vec3 &v = s.getOrigin();
float abs_distance = (v - kart->getXYZ()).length();
if (abs_distance < closest_distance)
{
closest_distance = abs_distance;
closest_id = n;
}
}
assert(closest_id != -1);
return closest_id;
} // getRescuePositionIndex
void Thread::optimizeManyPoints_MyParams(MatrixXd& constraints, int num_pts_between)
{
MatrixXd myPoints(constraints.rows(), 3); //really just going to throw this out...but this gives us curve params
vector<double> curvatures_resampled;
vector<double> torsions_resampled;
resamplePointsAndParams(myPoints, curvatures_resampled, torsions_resampled);
Matrix4d start_transform;
getStartTransform(start_transform);
optimizeManyPoints(constraints, num_pts_between, curvatures_resampled, torsions_resampled, start_transform);
}
//-----------------------------------------------------------------------------
AbstractKart *SoccerWorld::createKart(const std::string &kart_ident, int index,
int local_player_id, int global_player_id,
RaceManager::KartType kart_type,
PerPlayerDifficulty difficulty)
{
int posIndex = index;
int position = index+1;
if(race_manager->getLocalKartInfo(index).getSoccerTeam() == SOCCER_TEAM_RED)
{
if(index % 2 != 1) posIndex += 1;
}
else
{
if(index % 2 != 0) posIndex += 1;
}
btTransform init_pos = getStartTransform(posIndex);
AbstractKart *new_kart = new Kart(kart_ident, index, position, init_pos,
difficulty);
new_kart->init(race_manager->getKartType(index));
Controller *controller = NULL;
switch(kart_type)
{
case RaceManager::KT_PLAYER:
controller = new LocalPlayerController(new_kart,
StateManager::get()->getActivePlayer(local_player_id),
local_player_id);
m_num_players ++;
break;
case RaceManager::KT_NETWORK_PLAYER:
break; // Avoid compiler warning about enum not handled.
//controller = new NetworkController(kart_ident, position, init_pos,
// global_player_id);
//m_num_players++;
//break;
case RaceManager::KT_AI:
controller = loadAIController(new_kart);
break;
case RaceManager::KT_GHOST:
break;
case RaceManager::KT_LEADER:
break;
}
new_kart->setController(controller);
return new_kart;
} // createKart
void Thread::getActualEndTangent(Vector3d& endTan)
{
Matrix4d currTrans;
getStartTransform(currTrans);
currTrans.corner(Eigen::TopLeft,3,3) *= _length;
ThreadPiece* currPiece = threadList;
while (currPiece != NULL)
{
currTrans *= currPiece->_transform;
currPiece = currPiece->_next_segment;
}
endTan = currTrans.corner(Eigen::TopLeft,3,1);
}
/** Creates a kart, having a certain position, starting location, and local
* and global player id (if applicable).
* \param kart_ident Identifier of the kart to create.
* \param index Index of the kart.
* \param local_player_id If the kart is a player kart this is the index of
* this player on the local machine.
* \param global_player_id If the kart is a player kart this is the index of
* this player globally (i.e. including network players).
*/
AbstractKart *World::createKart(const std::string &kart_ident, int index,
int local_player_id, int global_player_id,
RaceManager::KartType kart_type,
PerPlayerDifficulty difficulty)
{
unsigned int gk = 0;
if (race_manager->hasGhostKarts())
gk = ReplayPlay::get()->getNumGhostKart();
int position = index+1;
btTransform init_pos = getStartTransform(index - gk);
AbstractKart *new_kart;
if (RewindManager::get()->isEnabled())
new_kart = new KartRewinder(kart_ident, index, position, init_pos,
difficulty);
else
new_kart = new Kart(kart_ident, index, position, init_pos, difficulty);
new_kart->init(race_manager->getKartType(index));
Controller *controller = NULL;
switch(kart_type)
{
case RaceManager::KT_PLAYER:
controller = new LocalPlayerController(new_kart,
StateManager::get()->getActivePlayer(local_player_id));
m_num_players ++;
break;
case RaceManager::KT_NETWORK_PLAYER:
controller = new NetworkPlayerController(new_kart);
m_num_players++;
break;
case RaceManager::KT_AI:
controller = loadAIController(new_kart);
break;
case RaceManager::KT_GHOST:
break;
case RaceManager::KT_LEADER:
break;
case RaceManager::KT_SPARE_TIRE:
break;
}
new_kart->setController(controller);
return new_kart;
} // createKart
bool Thread::resamplePointsAndParams(MatrixXd& resampled_points, vector<double>& curvatures, vector<double>& torsions)
{
curvatures.resize(resampled_points.rows());
torsions.resize(resampled_points.rows());
for (int i=0; i <curvatures.size(); i++)
{
curvatures[i] = torsions[i] = 0.0;
}
Matrix4d first_trans;
getStartTransform(first_trans);
first_trans.corner(Eigen::TopLeft,3,3)*= _length;
threadList->getPointsAndParams(resampled_points, 0.0, 1.0/((double)resampled_points.rows()-1), 0, first_trans, curvatures, torsions);
/*
for (int i=0; i < resampled_points.rows(); i++)
{
std::cout << "curvature: " << curvatures[i] << " torsion: " << torsions[i] << std::endl;
}
*/
}
unsigned int WorldWithRank::getRescuePositionIndex(AbstractKart *kart)
{
const int start_spots_amount = getTrack()->getNumberOfStartPositions();
assert(start_spots_amount > 0);
float largest_accumulated_distance_found = -1;
int furthest_id_found = -1;
for(int n=0; n<start_spots_amount; n++)
{
const btTransform &s = getStartTransform(n);
const Vec3 &v=s.getOrigin();
float accumulated_distance = .0f;
bool spawn_point_clear = true;
for(unsigned int k=0; k<getCurrentNumKarts(); k++)
{
if(kart->getWorldKartId()==k) continue;
float abs_distance2 = (getKart(k)->getXYZ()-v).length2();
const float CLEAR_SPAWN_RANGE2 = 5*5;
if( abs_distance2 < CLEAR_SPAWN_RANGE2)
{
spawn_point_clear = false;
break;
}
accumulated_distance += sqrt(abs_distance2);
}
if(accumulated_distance > largest_accumulated_distance_found &&
spawn_point_clear)
{
furthest_id_found = n;
largest_accumulated_distance_found = accumulated_distance;
}
}
assert(furthest_id_found != -1);
return furthest_id_found;
} // getRescuePositionIndex
/** Creates a kart, having a certain position, starting location, and local
* and global player id (if applicable).
* \param kart_ident Identifier of the kart to create.
* \param index Index of the kart.
* \param local_player_id If the kart is a player kart this is the index of
* this player on the local machine.
* \param global_player_id If the kart is a player kart this is the index of
* this player globally (i.e. including network players).
*/
AbstractKart *World::createKart(const std::string &kart_ident, int index,
int local_player_id, int global_player_id,
RaceManager::KartType kart_type,
const PlayerDifficulty *difficulty)
{
int position = index+1;
btTransform init_pos = getStartTransform(index);
AbstractKart *new_kart = new Kart(kart_ident, index, position, init_pos,
difficulty);
new_kart->init(race_manager->getKartType(index));
Controller *controller = NULL;
switch(kart_type)
{
case RaceManager::KT_PLAYER:
controller = new PlayerController(new_kart,
StateManager::get()->getActivePlayer(local_player_id),
local_player_id);
m_num_players ++;
break;
case RaceManager::KT_NETWORK_PLAYER:
controller = new NetworkPlayerController(new_kart,
StateManager::get()->getActivePlayer(local_player_id));
m_num_players++;
break;
case RaceManager::KT_AI:
controller = loadAIController(new_kart);
break;
case RaceManager::KT_GHOST:
break;
case RaceManager::KT_LEADER:
break;
}
new_kart->setController(controller);
return new_kart;
} // createKart
/** Creates a kart, having a certain position, starting location, and local
* and global player id (if applicable).
* \param kart_ident Identifier of the kart to create.
* \param index Index of the kart.
* \param local_player_id If the kart is a player kart this is the index of
* this player on the local machine.
* \param global_player_id If the akrt is a player kart this is the index of
* this player globally (i.e. including network players).
* \param init_pos The start XYZ coordinates.
*/
AbstractKart *ProfileWorld::createKart(const std::string &kart_ident, int index,
int local_player_id, int global_player_id,
RaceManager::KartType type,
PerPlayerDifficulty difficulty)
{
btTransform init_pos = getStartTransform(index);
Kart *new_kart = new KartWithStats(kart_ident,
/*world kart id*/ index,
/*position*/ index+1,
init_pos, difficulty);
new_kart->init(RaceManager::KT_AI);
Controller *controller = loadAIController(new_kart);
new_kart->setController(controller);
// Create a camera for the last kart (since this way more of the
// karts can be seen.
if (index == (int)race_manager->getNumberOfKarts()-1)
{
// The camera keeps track of all cameras and will free them
Camera::createCamera(new_kart);
}
return new_kart;
} // createKart
void Thread::optimizeManyPoints(MatrixXd& constraints, int num_pts_between, vector<double>&curvatures_resampled, vector<double>& torsions_resampled, Matrix4d& start_transform)
{
int num_constraints = constraints.rows()-1;
int num_pieces_reopt = num_pts_between*num_constraints;
double length_per_piece = 1.0/( num_pieces_reopt);
//opt_params_many_points.transform_back = _transform_to_start;
opt_params_many_points.num_segments = num_pieces_reopt;
opt_params_many_points.length_per_segment = length_per_piece;
opt_params_many_points.points.resize(num_constraints);
opt_params_many_points.orig_params_each_piece = new NEWMAT::ColumnVector(2*num_pieces_reopt+3);
opt_params_many_points.num_pts_between = num_pts_between;
opt_params_many_points.gravity_multipler = (GRAVITY_CONSTANT)*_length;
opt_params_many_points.total_length = _length;
for (int i=0; i < num_constraints; i++)
{
for (int j=0; j < num_pts_between; j++)
{
int piece_num = i*num_pts_between+j;
opt_params_many_points.orig_params_each_piece->element(2*piece_num) = curvatures_resampled[i];
opt_params_many_points.orig_params_each_piece->element(2*piece_num+1) = torsions_resampled[i];
}
}
double eulerZ, eulerY, eulerX;
eulerAnglesFramTransform(start_transform.corner(Eigen::TopLeft,3,3), eulerZ, eulerY, eulerX);
opt_params_many_points.orig_params_each_piece->element(2*num_pieces_reopt) = eulerZ;
opt_params_many_points.orig_params_each_piece->element(2*num_pieces_reopt+1) = eulerY;
opt_params_many_points.orig_params_each_piece->element(2*num_pieces_reopt+2) = eulerX;
for (int i=0; i < num_constraints; i++)
{
opt_params_many_points.points[i] = ((constraints.block((i+1),0,1,3) - constraints.block(0,0,1,3))/_length).transpose();
}
NEWMAT::ColumnVector x_sol;
optimize_FDNLF(2*opt_params_many_points.num_segments+3, energyEvalFunctionManyPoints, energyEvalFunctionManyPoints_init, x_sol);
std::cout << "done re-optimizing" << std::endl;
delete opt_params_many_points.orig_params_each_piece;
//set thread params
_positions[0] = constraints.block(0,0,1,3).transpose();
transformFromEulerAngles(_transform_to_start, x_sol(2*num_pieces_reopt+1), x_sol(2*num_pieces_reopt+2));
_translate_to_start = Matrix4d::Identity();
_translate_to_start.corner(Eigen::TopRight,3,1) = _positions[0];
_tangents[0] = _transform_to_start.corner(Eigen::TopLeft,3,1);
//_tangents[0] = orig_thread->_tangents[0];
_angle_first_rot = x_sol(2*num_pieces_reopt+3);
inverseTransform(_transform_to_start, _transform_to_unit);
Matrix4d transform_back;
getStartTransform(transform_back);
transform_back.corner(Eigen::TopLeft,3,3) *= _length;
threadList = new ThreadPiece(x_sol(1), x_sol(2), length_per_piece);
ThreadPiece* currPiece = threadList;
for (int i=1; i < num_pieces_reopt; i++)
{
currPiece->addSegmentAfter(new ThreadPiece(x_sol(2*i+1), x_sol(2*i+2), length_per_piece));
transform_back *= currPiece->_transform;
currPiece = currPiece->_next_segment;
}
//_positions[1] = transform_back.corner(Eigen::TopRight,3,1);
// _positions[1] = orig_thread->_positions[1];
// _tangents[1] = orig_thread->_tangents[1];
// _tangents[1].normalize();
// _tangents[0].normalize();
_positions[1] = transform_back.corner(Eigen::TopRight,3,1);
_tangents[1] = transform_back.corner(Eigen::TopLeft,3,1);
_tangents[1].normalize();
//printThreadInfo();
}
//-----------------------------------------------------------------------------
AbstractKart *SoccerWorld::createKart(const std::string &kart_ident, int index,
int local_player_id, int global_player_id,
RaceManager::KartType kart_type,
PerPlayerDifficulty difficulty)
{
int cur_red = getTeamNum(SOCCER_TEAM_RED);
int cur_blue = getTeamNum(SOCCER_TEAM_BLUE);
int pos_index = 0;
int position = index + 1;
SoccerTeam team = SOCCER_TEAM_BLUE;
if (kart_type == RaceManager::KT_AI)
{
if (index < m_red_ai)
team = SOCCER_TEAM_RED;
else
team = SOCCER_TEAM_BLUE;
m_kart_team_map[index] = team;
}
else
{
int rm_id = index -
(race_manager->getNumberOfKarts() - race_manager->getNumPlayers());
assert(rm_id >= 0);
team = race_manager->getKartInfo(rm_id).getSoccerTeam();
m_kart_team_map[index] = team;
}
// Notice: In blender, please set 1,3,5,7... for blue starting position;
// 2,4,6,8... for red.
if (team == SOCCER_TEAM_BLUE)
{
pos_index = 1 + 2 * cur_blue;
}
else
{
pos_index = 2 + 2 * cur_red;
}
btTransform init_pos = getStartTransform(pos_index - 1);
m_kart_position_map[index] = (unsigned)(pos_index - 1);
AbstractKart *new_kart = new Kart(kart_ident, index, position, init_pos,
difficulty);
//difficulty, team == SOCCER_TEAM_BLUE ?
//video::ERT_BLUE : video::ERT_RED);
new_kart->init(race_manager->getKartType(index));
Controller *controller = NULL;
switch(kart_type)
{
case RaceManager::KT_PLAYER:
controller = new LocalPlayerController(new_kart,
StateManager::get()->getActivePlayer(local_player_id));
m_num_players ++;
break;
case RaceManager::KT_NETWORK_PLAYER:
break; // Avoid compiler warning about enum not handled.
//controller = new NetworkController(kart_ident, position, init_pos,
// global_player_id);
//m_num_players++;
//break;
case RaceManager::KT_AI:
controller = loadAIController(new_kart);
break;
case RaceManager::KT_GHOST:
break;
case RaceManager::KT_LEADER:
break;
}
new_kart->setController(controller);
return new_kart;
} // createKart
