bool RPCCamera::ToVector(double pix_x, double pix_y, MyVector3d &v, MyPoint3d &p) const
{
GDALRPCInfo rpcInfo = rpcModel.rpcInfo;
double hup = rpcInfo.dfHEIGHT_OFF + rpcInfo.dfHEIGHT_SCALE * 0.9;
double hdown = rpcInfo.dfHEIGHT_OFF - rpcInfo.dfHEIGHT_SCALE * 0.9;
double hup_lon, hup_lat, hdown_lon, hdown_lat;
ToGeodetic(pix_x, pix_y, hup, hup_lon, hup_lat);
ToGeodetic(pix_x, pix_y, hdown, hdown_lon, hdown_lat);
double hup_x, hup_y, hup_z, hdown_x, hdown_y, hdown_z;
geodetic_to_cartesian(hup_lon, hup_lat, hup, hup_x, hup_y, hup_z);
geodetic_to_cartesian(hdown_lon, hdown_lat, hdown, hdown_x, hdown_y, hdown_z);
v = MyVectorSub(MyVector3d(hdown_x, hdown_y, hdown_z),
MyVector3d(hup_x, hup_y, hup_z));
MyVectorNormalize(v);
p.p[0] = hup_x - 10000 * v.p[0];
p.p[1] = hup_y - 10000 * v.p[1];
p.p[2] = hup_z - 10000 * v.p[2];
return true;
}
//获取指定时间的命令，如果超前，返回停止状态
RobotTracker::rcommand RobotTracker::get_command(double time)
{
rcommand c={ 0.0, MyVector3d(0.0, 0.0, 0.0) };
if (cs.empty() || cs[0].timestamp > time)
{
return c;
}
uint i;
for(i = 1; i < cs.size(); i++)
{
if (cs[i].timestamp > time) break;
}
return cs[i-1];
}
