//|
/// void LC3EnemyLook(struct SquareData *felt, struct LC3Brain *mem)
void LC3EnemyLook(struct SquareData *felt, struct LC3Brain *mem) {
int f;
/* Bask fjender !! */
if (felt[1].Team+felt[2].Team+felt[3].Team+felt[4].Team) {
for (f = 1 ; f <= 4 ; f++) {
if (felt[f].Team) {
mem->xdest = mem->xpos+xdir(f);
mem->ydest = mem->ypos+ydir(f);
mem->status = LC_Status_Searching;
mem->num = LCDanger;
break;
}
}
}
}
//*****************************************************************************
// CONSTRUCTOR: ossimLsrSpace
//
// Constructs the space given origin, and Y and Z ECEF directions. The int
// argument is a place holder only and not used.
//
//*****************************************************************************
ossimLsrSpace::ossimLsrSpace(const ossimEcefPoint& origin,
int /* x_not_provided_space_holder */,
const ossimEcefVector& y_dir_ecf_vec,
const ossimEcefVector& z_dir_ecf_vec)
: theOrigin (origin)
{
//***
// Compute the remaining axis given first two:
//***
ossimColumnVector3d ydir (y_dir_ecf_vec.data().unit());
ossimColumnVector3d zdir (z_dir_ecf_vec.data().unit());
ossimColumnVector3d xdir (ydir.cross(zdir));
//***
// Fill the rotation matrix:
//***
theLsrToEcefRotMatrix = ossimMatrix3x3::create(xdir[0], ydir[0], zdir[0],
xdir[1], ydir[1], zdir[1],
xdir[2], ydir[2], zdir[2]);
}
/// void LC3FoodLook(struct SquareData *felt, struct LC3Brain *mem)
void LC3FoodLook(struct SquareData *felt, struct LC3Brain *mem) {
int f;
/* Kig efter mad */
if (felt[1].NumFood+felt[2].NumFood+felt[3].NumFood+felt[4].NumFood) {
for (f = 1 ; f <= 4 ; f++) {
if (felt[f].NumFood > felt[f].NumAnts) {
mem->xdest = mem->xpos+xdir(f);
mem->ydest = mem->ypos+ydir(f);
break;
}
}
}
if (felt->NumFood > felt->NumAnts) {
mem->xdest = mem->xpos;
mem->ydest = mem->ypos;
mem->num = felt->NumFood-felt->NumAnts;
mem->status = LC_Status_FoundFood;
}
}
void StereoReconstructor::computeCoordinate(std::string corners, std::string coord, int x, int y) {
//读取所有的角点
std::ifstream file(corners.c_str());
std::vector<cv::Point3f> corner;
corner.reserve(x*y);
cv::Point3f point;
while (file >> point.x >> point.y >> point.z) {
corner.push_back(point);
}
file.close();
//计算x方向
cv::Point3f pd = corner[x - 1] - corner[0];
cv::Point3f xdir(0, 0, 0);
for (int i = 0; i < y; i++) {
std::vector<cv::Point3f> tps;
//提取x方向的点
for (int j = 0; j < x; j++) {
cv::Point3f tp = corner[i*x + j];
tps.push_back(tp);
}
cv::Point3f td = Utilities::lineFit(tps);
xdir += td;
}
xdir.x = xdir.x / y;
xdir.y = xdir.y / y;
xdir.z = xdir.z / y;
if (pd.dot(xdir) < 0) {
xdir = -xdir;
}
double xv[] = {xdir.x, xdir.y, xdir.z};
double normal = sqrt(xv[0] * xv[0] + xv[1] * xv[1] + xv[2] * xv[2]);
xv[0] /= normal;
xv[1] /= normal;
xv[2] /= normal;
//计算y方向
cv::Point3f pyd = corner[x] - corner[0];
cv::Point3f ydir(0, 0, 0);
for (int i = 0; i < x; i++) {
std::vector<cv::Point3f> tps;
//提取y方向的点
for (int j = 0; j < y; j++) {
cv::Point3f tp = corner[j*x + i];
tps.push_back(tp);
}
cv::Point3f td = Utilities::lineFit(tps);
ydir += td;
}
ydir.x = ydir.x / x;
ydir.y = ydir.y / x;
ydir.z = ydir.z / x;
if (pyd.dot(ydir) < 0) {
ydir = -ydir;
}
double yv[] = {ydir.x, ydir.y, ydir.z};
normal = sqrt(yv[0] * yv[0] + yv[1] * yv[1] + yv[2] * yv[2]);
yv[0] /= normal;
yv[1] /= normal;
yv[2] /= normal;
//计算坐标系
double zv[3];
zv[0] = xv[1] * yv[2] - xv[2] * yv[1];
zv[1] = xv[2] * yv[0] - xv[0] * yv[2];
zv[2] = xv[0] * yv[1] - xv[1] * yv[0];
//保存到坐标系文件
std::ofstream coordinate(coord);
coordinate << corner[0].x << '\t' << corner[0].y << '\t' << corner[0].z << std::endl;
coordinate << xv[0] << '\t' << xv[1] << '\t' << xv[2] << std::endl;
coordinate << yv[0] << '\t' << yv[1] << '\t' << yv[2] << std::endl;
coordinate << zv[0] << '\t' << zv[1] << '\t' << zv[2] << std::endl;
coordinate.close();
}
