glm::fquat Lerp(const glm::fquat &v0, const glm::fquat &v1, float alpha)
{
glm::vec4 start = Vectorize(v0);
glm::vec4 end = Vectorize(v1);
glm::vec4 interp = glm::mix(start, end, alpha);
printf("alpha: %f, (%f, %f, %f, %f)\n", alpha, interp.w, interp.x, interp.y, interp.z);
interp = glm::normalize(interp);
return glm::fquat(interp.w, interp.x, interp.y, interp.z);
}
void inertialParametersVectorLoop(const UndirectedTree & undirected_tree,
Eigen::VectorXd & parameters_vector)
{
for(int i=0; i < (int)undirected_tree.getNrOfLinks(); i++ ) {
parameters_vector.segment(i*10,10) = Vectorize(undirected_tree.getLink(i)->getInertia());
}
}
void TrainingSet::Concatenation(Mat & KEY, Point2d* VALUE, float* HighMean, int & count)
{
for (int row = 0; row < m_PatchNo_row; row++)
{
for (int col = 0; col < m_PatchNo_col; col++)
{
Mat output;
int MatSize=61;
Vectorize(row, col, output,MatSize);
float x = MatMean(output);
float mean = Std(output, x);
//output = output / mean;
for (int n = 0; n < MatSize; n++)
{
KEY.at<float>(count, n) = output.at<float>(0, n)/mean;
}
HighMean[count] = mean;
VALUE[count] = Point2d(row, col);
count++;
//Mat dsttemp = KEY.row(count); //MΪĿµÄ¾ØÕó n*m
//output.copyTo(dsttemp);
//float y = KEY.at<float>(0, 1);
/*if (row == 42 && col == 193 && count == 466609)
{
int errpr = 0;
Mat a = m_highResPatches->GetPatch(row + 1, col + 1) / mean;
}*/
/*Mat a = m_highResPatches->GetPatch(row + 1, col + 1) / mean;
if (row==310&&col==440)
{
int errpr = 0;
}
Mat b(1, 25, CV_32FC3);
b = VALUE(Rect(0, count, 25, 1));*/
/*int count = 0;
for (int row = 0; row < 5; row++)
{
for (int col = 0; col < 5; col++)
{
for (int n = 0; n < 3; n++)
{
b.at<Vec3f>(0, count)[n] = a.at<Vec3f>(row, col)[n];
}
count++;
}
}*/
//VALUE.push_back(m_highResPatches->GetPatch(row + 1, col + 1) / mean);
//VALUE(Rect(count,0,1,25))=b;
/*if (count == 466609)
{
int n = 0;
}*/
}
}
}
void TreeInertialParameters::updateParams()
{
Segment seg;
tree_param.resize(10*ns);
for(unsigned int i = 0; i < ns; i++ ) {
seg = seg_vector[i]->second.segment;
tree_param.segment(i*10,10) = Vectorize(seg.getInertia());
}
}
void inertialParametersVectorLoopFakeLinks(const UndirectedTree & undirected_tree,
Eigen::VectorXd & parameters_vector,
std::vector< std::string > fake_links_names)
{
int real_index_loop = 0;
for(int i=0; i < (int)undirected_tree.getNrOfLinks(); i++ ) {
if( std::find(fake_links_names.begin(), fake_links_names.end(), undirected_tree.getLink(i)->getName()) == fake_links_names.end() ) {
parameters_vector.segment(real_index_loop*10,10) = Vectorize(undirected_tree.getLink(i)->getInertia());
real_index_loop++;
}
}
}
phys_obj* ai_agent::GetClosestSpriteID(phys_obj* physPtr, bool findEnemy = false) {
long tileNum = 0;
float dir = 0.0f;
int range = (physPtr->viewdistance/3);
for (range; range < physPtr->viewdistance; range+=physPtr->viewdistance/3) {
for (dir = 0; dir < atan(1.0f)*4; dir+= atan(1.0f)/2) {
for (long lr = 1; lr >= -1; lr-=2) {
tileNum = ptrWorld->ptrPhysics->getRelativeTile(physPtr,Vectorize(physPtr->vector,dir*lr),range);
if (tileNum > 0 && tileNum < this->ptrWorld->Map->WorldInfo.total_tiles) {
//ptrWorld->ptrPhysics->addToTileMap(this->ptrWorld->Map->pred_map,tileNum,Ent->ID);
//this->addToTileMap(this->ptrWorld->Map->pred_map,tileNum,Ent->ID);
if (this->ptrWorld->Map->tile_map[tileNum].Occupied) {
for (int i = 0; i < this->ptrWorld->Map->OccupyMax; i++) {
TempEnt = getSprite(this->ptrWorld->Map->tile_map[tileNum].OccupyData[i]);
if (physPtr->team == TempEnt->team) {
if (!findEnemy) {
return TempEnt;
}
} else {
if (findEnemy) {
return TempEnt;
}
}
}
}
}
}
}
}
return new phys_obj(-2);
}
phys_obj* ai_agent::GetClosestVisibleSprite2(phys_obj* physPtr, bool findEnemy = false) {
//because of the order of tile checking, the first entity found is basicly the closest, so it can instantly return and not
//waste time calculating and comparing the distance of every entitity, fairly efficient, but has holes put close, more resoltuion needed
//TEMP FUNCTION DISABLER
//return new phys_obj(-2);
long tileNum = 0;
Tile* ptrTile;
//Tile* ptrTile2;
//Tile* ptrTile3;
long tempnum = 0;
float dir = 0.0f;
int range = (physPtr->viewdistance/3);
for (range; range < physPtr->viewdistance; range+=physPtr->viewdistance/3) {
for (dir = 0; dir < atan(1.0f)*4; dir+= atan(1.0f)/2) {
for (long lr = 1; lr >= -1; lr-=2) {
tileNum = ptrWorld->ptrPhysics->getRelativeTile(physPtr,Vectorize(physPtr->vector,dir*lr),range);
if (tileNum > 0 && tileNum < this->ptrWorld->Map->WorldInfo.total_tiles) {
while(this->ptrWorld->lock_tile_map) { }
//ptrTile2 = &this->ptrWorld->Map->tile_map_temp[tileNum];
//ptrTile3 = &this->ptrWorld->Map->tile_map_swap[tileNum];
//this->addToTileMap(this->ptrWorld->Map->pred_map,tileNum,Ent->ID);
//this->addToTileMap(this->ptrWorld->Map->pred_map,tileNum,Ent->ID);
ptrTile = &this->ptrWorld->Map->tile_map[tileNum];
if (ptrTile->Occupied) {
//long od = ptrTile->OccupyData;
//this->ptrWorld->los_checks++;
//while(this->ptrWorld->lock_tile_map) { }
//int tempp = od.size();
for (int i = 0; i < ptrTile->OccupyCount; i++) {
tempnum = ptrTile->OccupyData[i];
if (tempnum != -2) {
TempEnt = getSprite(tempnum);
if (physPtr->team == TempEnt->team) {
if (!findEnemy) {
if (this->LOSCheck(physPtr,TempEnt)) {
return TempEnt;
}
}
} else {
if (findEnemy) {
if (this->LOSCheck(physPtr,TempEnt)) {
return TempEnt;
}
}
}
}
}
}
}
}
}
}
return new phys_obj(-2);
}
