//------------------------------------------------------------------------------
// Write a bitmap (BMP) format file
//------------------------------------------------------------------------------
bool Image::writeFileBMP(const char* const filename, const char* const path)
{
static const unsigned int BITMAPFILE_SIZE = (MAX_PATH_LEN+MAX_FILENAME_LEN);
char bitmapFile[BITMAPFILE_SIZE];
bitmapFile[0] = '\0';
// append path name
const char* p1 = path;
if (p1 != nullptr && std::strlen(path) > 0) {
base::utStrcat(bitmapFile, sizeof(bitmapFile), p1);
base::utStrcat(bitmapFile, sizeof(bitmapFile), "/");
}
// append file name
const char* p2 = filename;
if (p2 != nullptr && std::strlen(p2) > 0) {
base::utStrcat(bitmapFile, sizeof(bitmapFile), p2);
}
// Do we have a full path name?
if (std::strlen(bitmapFile) <= 1) {
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "Image::writeFileBMP(): invalid file name: " << bitmapFile << std::endl;
}
return false;
}
// Create the output stream
auto fout = new std::ofstream();
// Open the output file
fout->open(bitmapFile, std::ios::out | std::ios::binary);
if ( !(fout->is_open()) ) {
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "Image::writeFileBMP(): unable to open bitmap file: " << bitmapFile << std::endl;
}
return false;
}
// BITMAPFILEHEADER
unsigned short bfType(0);
unsigned int bfSize(0);
unsigned short bfReserved1(0);
unsigned short bfReserved2(0);
unsigned int bfOffBits(0);
unsigned int bitmapFileHdrSize =
sizeof(bfType) + sizeof(bfSize) + sizeof(bfReserved1) + sizeof(bfReserved2) + sizeof(bfOffBits);
// Number of bytes per row of pixels
size_t widthBytes = getWidth() * getNumComponents();
// Offset to bitmap data
unsigned int offset = bitmapFileHdrSize + sizeof(BITMAPINFOHEADER_X);
// Image size
unsigned int isize = getHeight() * static_cast<unsigned int>(widthBytes);
// File size (active bytes)
unsigned int size = isize + offset;
// File size (4 byte words)
unsigned int sizew = (size + 3)/4;
// Total file size (with padding)
unsigned int tsize = sizew*4;
// Number of padding bytes at the end if the file to give a even word boundary
unsigned int filePadding = tsize - size;
// ---
// Write the bitmap file header (BITMAPFILEHEADER)
// ---
bfType = 0x4D42;
bfSize = tsize;
bfReserved1 = 0;
bfReserved2 = 0;
bfOffBits = offset;
fout->write(reinterpret_cast<char*>(&bfType), sizeof(bfType));
fout->write(reinterpret_cast<char*>(&bfSize), sizeof(bfSize));
fout->write(reinterpret_cast<char*>(&bfReserved1), sizeof(bfReserved1));
fout->write(reinterpret_cast<char*>(&bfReserved2), sizeof(bfReserved2));
fout->write(reinterpret_cast<char*>(&bfOffBits), sizeof(bfOffBits));
// ---
// Write the bitmap file info
// ---
BITMAPINFOHEADER_X bmfi;
bmfi.biSize = sizeof(BITMAPINFOHEADER_X);
bmfi.biWidth = getWidth();
bmfi.biHeight = getHeight();
bmfi.biPlanes = 1;
bmfi.biBitCount = getNumComponents() * 8;
bmfi.biCompression = BI_RGB;
bmfi.biSizeImage = 0;
bmfi.biXPelsPerMeter = getXResolutionPPM();
bmfi.biYPelsPerMeter = getYResolutionPPM();
bmfi.biClrUsed = 0;
bmfi.biClrImportant = 0;
fout->write(reinterpret_cast<char*>(&bmfi), sizeof(bmfi));
// ---
// Write the pixel bit map
// ---
{
const GLubyte* bmap = getPixels();
for (unsigned int i = 0; i < getHeight(); i++) {
const GLubyte* p = bmap + (i * widthBytes);
fout->write(reinterpret_cast<const char*>(p), width* PIXEL_SIZE);
}
if (filePadding > 0) {
unsigned int padding = 0;
fout->write(reinterpret_cast<char*>(&padding), filePadding);
}
}
// close the file
fout->close();
delete fout;
fout = nullptr;
return true;
}
//------------------------------------------------------------------------------
// closeConnection() -- request that the connection is closed (shutdown)
//------------------------------------------------------------------------------
bool TcpHandler::closeConnection()
{
bool success = true;
#if defined(WIN32)
if (::closesocket(socketNum) == SOCKET_ERROR) {
#else
if (::shutdown(socketNum, SHUT_RDWR) == SOCKET_ERROR) {
#endif
std::perror("TcpHandler::closeConnection(): error! \n");
success = false;
}
connected = false;
connectionTerminated = true;
return BaseClass::closeConnection() && success;
}
// -------------------------------------------------------------
// sendData() -- Send data to our connected TCP socket
// -------------------------------------------------------------
bool TcpHandler::sendData(const char* const packet, const int size)
{
if (!isConnected() || hasBeenTerminated()) return false;
if (socketNum == INVALID_SOCKET) return false;
int result = ::send(socketNum, packet, size, 0);
if (result == SOCKET_ERROR) {
connected = false;
connectionTerminated = true;
#if defined(WIN32)
int err = WSAGetLastError();
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "TcpHandler::sendData(): sendto error: " << err << " hex=0x" << std::hex << err << std::dec << std::endl;
}
#else
std::perror("TcpHandler::sendData(): sendto error msg");
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "TcpHandler::sendData(): sendto error result: " << result << std::endl;
}
#endif
return false;
}
return true;
}
// -------------------------------------------------------------
// recvData() -- Receive data from our connected TCP socket
// -------------------------------------------------------------
unsigned int TcpHandler::recvData(char* const packet, const int maxSize)
{
if (!isConnected() || hasBeenTerminated()) return 0;
if (socketNum == INVALID_SOCKET) return 0;
unsigned int n = 0; // default return value (no data)
// Try to receive the data
int result = ::recv(socketNum, packet, maxSize, 0);
// Did we received any data?
if (result > 0) {
// We've received data -- all is well.
n = static_cast<unsigned int>(result);
}
// Did we receive a zero?
else if (result == 0) {
// Received a zero -- connection closed by other side
closeConnection();
}
// Do we have an error code?
else if (result < 0) {
// For error conditions, check for non-blocking and adjust result
// to indicate there is no error
if (errno != EAGAIN && errno != EWOULDBLOCK) {
// Error condition! Close the conntection
perror("TcpHandler::recvData(): ");
closeConnection();
}
}
return n;
}
} // End Basic namespace
//------------------------------------------------------------------------------
// Read a bitmap (BMP) format file
//------------------------------------------------------------------------------
bool Image::readFileBMP(const char* const filename, const char* const path)
{
static const unsigned int BITMAPFILE_SIZE = (MAX_PATH_LEN + MAX_FILENAME_LEN);
char bitmapFile[BITMAPFILE_SIZE];
bitmapFile[0] = '\0';
// append path name
const char* p1 = path;
if (p1 != nullptr && std::strlen(p1) > 0) {
base::utStrcat(bitmapFile, sizeof(bitmapFile), p1);
base::utStrcat(bitmapFile, sizeof(bitmapFile), "/");
}
// append file name
const char* p2 = filename;
if (p2 != nullptr && std::strlen(p2) > 0) {
base::utStrcat(bitmapFile, sizeof(bitmapFile), p2);
}
// Do we have a full path name?
if (std::strlen(bitmapFile) <= 1) {
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "Image::readFileBMP(): invalid file name: " << bitmapFile << std::endl;
}
return false;
}
if (isMessageEnabled(MSG_INFO)) {
std::cout << "Image: Loading file: " << filename << std::endl;
}
FILE* fp = std::fopen(bitmapFile,"rb");
if (fp == nullptr) {
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "Image::readFileBMP: unable to open bitmap file: " << bitmapFile << std::endl;
}
return false;
}
// Big or little ending? Swap if we're a big endian arch
bool swap = checkSwap();
// Read the bitmap file header (BITMAPFILEHEADER)
char bfType[2];
uint32_t bfSize(0);
uint16_t bfReserved1(0);
uint16_t bfReserved2(0);
uint32_t bfOffBits(0);
//unsigned int bitmapFileHdrSize =
// sizeof(bfType) + sizeof(bfSize) + sizeof(bfReserved1) + sizeof(bfReserved2) + sizeof(bfOffBits);
size_t nItemsRead(0);
nItemsRead = std::fread(&bfType, sizeof(char), 2, fp);
nItemsRead = std::fread(&bfSize, sizeof(bfSize), 1, fp);
if (swap) bfSize = convertUInt32(bfSize);
nItemsRead = std::fread(&bfReserved1, sizeof(bfReserved1), 1, fp);
if (swap) bfReserved1 = convertUInt16(bfReserved1);
nItemsRead = std::fread(&bfReserved2, sizeof(bfReserved2), 1, fp);
if (swap) bfReserved2 = convertUInt16(bfReserved2);
nItemsRead = std::fread(&bfOffBits, sizeof(bfOffBits), 1, fp);
if (swap) bfOffBits = convertUInt32(bfOffBits);
if (bfType[0] != 'B' || bfType[1] != 'M') {
// Not a bitmap file
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "Image::readFileBMP(1): invalid bitmap file: " << bitmapFile << std::endl;
}
std::fclose(fp);
return false;
}
// Read the bitmap file info
BITMAPINFOHEADER_X bmfi;
nItemsRead = std::fread(&bmfi, sizeof(BITMAPINFOHEADER_X), 1, fp);
if (swap) {
bmfi.biSize = convertUInt32(bmfi.biSize);
bmfi.biWidth = convertInt32(bmfi.biWidth);
bmfi.biHeight = convertInt32(bmfi.biHeight);
bmfi.biPlanes = convertUInt16(bmfi.biPlanes);
bmfi.biBitCount = convertUInt16(bmfi.biBitCount);
bmfi.biCompression = convertUInt32(bmfi.biCompression);
bmfi.biSizeImage = convertUInt32(bmfi.biSizeImage);
bmfi.biXPelsPerMeter = convertInt32(bmfi.biXPelsPerMeter);
bmfi.biYPelsPerMeter = convertInt32(bmfi.biYPelsPerMeter);
bmfi.biClrUsed = convertUInt32(bmfi.biClrUsed);
bmfi.biClrImportant = convertUInt32(bmfi.biClrImportant);
}
if (bmfi.biSize != sizeof(BITMAPINFOHEADER_X) || bmfi.biPlanes != 1) {
// Not a bitmap file
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "Image::readFileBMP(2): invalid bitmap file: " << bitmapFile << std::endl;
}
std::fclose(fp);
return false;
}
// set width & height (truncates to powers of two)
setWidth(bmfi.biWidth);
setHeight(bmfi.biHeight);
// set resolution
setXResolutionPPM(bmfi.biXPelsPerMeter);
setYResolutionPPM(bmfi.biYPelsPerMeter);
// Read the colors
GLubyte* bmap = nullptr;
if (bmfi.biBitCount == 24) {
setNumComponents(3);
setFormat(GL_BGR_EXT);
bmap = readRgbValuesBMP(fp, bfOffBits, &bmfi);
}
else if (bmfi.biBitCount == 8){
setNumComponents(3);
setFormat(GL_BGR_EXT);
bmap = readColorValuesBMP(fp, bfOffBits, &bmfi);
}
else if (bmfi.biBitCount == 4){
if (isMessageEnabled(MSG_WARNING)) {
std::cerr << "Image::readFileBMP: can not load a 16 color bitmap!" << std::endl;
}
}
if (bmap == nullptr) {
if (isMessageEnabled(MSG_WARNING)) {
std::cerr << "Image::readFileBMP(3): invalid bitmap file: " << bitmapFile << std::endl;
}
}
// close the file
std::fclose(fp);
// Set the pixel bit map
if (bmap != nullptr) setPixels(bmap);
return (bmap != nullptr);
}
//------------------------------------------------------------------------------
// irRequestSignature() -- Send an IR query packet at all active players to request an IR signature
//------------------------------------------------------------------------------
void IrSeeker::irRequestSignature(IrQueryMsg* const irQuery)
{
// Need something to store the required data for the IR signatures and someone to send to
Tdb* tdb0 = getCurrentTDB();
Player* ownship = getOwnship();
if (irQuery == 0 || tdb0 == 0 || ownship == 0) {
// Clean up and leave
if (tdb0 != 0) tdb0->unref();
return;
}
// ---
// Compute gimbal boresight data for our targets
// ---
// FAB - cannot use ownHdgOnly
unsigned int ntgts = tdb0->computeBoresightData();
if (ntgts > MAX_PLAYERS) ntgts = MAX_PLAYERS;
// ---
// If we have targets
// ---
const osg::Vec3d* losG = tdb0->getGimbalLosVectors();
if (ntgts > 0 && losG != 0) {
// Fetch the required data arrays from the TargetDataBlock
const double* ranges = tdb0->getTargetRanges();
const double* rngRates = tdb0->getTargetRangeRates();
const double* anglesOffBoresight = tdb0->getBoresightErrorAngles();
const osg::Vec3d* losO2T = tdb0->getLosVectors();
const osg::Vec3d* losT2O = tdb0->getTargetLosVectors();
Player** targets = tdb0->getTargets();
LCreal maximumRange = irQuery->getMaxRangeNM()*Basic::Distance::NM2M;
// ---
// Send query packets to the targets
// ---
for (unsigned int i = 0; i < ntgts; i++) {
// filter on sensor max range
// can't filter on sensor range in processPlayers - different sensors can have different max range
if (maximumRange > 0.0 && ranges[i] > maximumRange)
continue;
// Get a free query packet
lcLock(freeQueryLock);
IrQueryMsg* query = freeQueryStack.pop();
lcUnlock(freeQueryLock);
if (query == 0) {
query = new IrQueryMsg();
//if (ownship->getID() != 1) {
// static tcnt = 0;
// tcnt++;
//if (isMessageEnabled(MSG_INFO)) {
// std::cout << "new IrQueryMsg(" << this << "): " << tcnt << ", inused: " << inUseEmQueue.entries() << ", em = " << em << std::endl;
//}
//}
}
// Send the IR query message to the other player
if (query != 0) {
// a) Copy the template query msg
*query = *irQuery;
// b) Set target unique data
query->setGimbal(this);
query->setOwnship(ownship);
query->setRange( LCreal(ranges[i]) );
query->setLosVec( losO2T[i] );
query->setTgtLosVec( losT2O[i] );
query->setRangeRate( LCreal(rngRates[i]) );
query->setTarget(targets[i]);
query->setAngleOffBoresight( LCreal(anglesOffBoresight[i]) );
query->setGimbalAzimuth( LCreal(getAzimuth()) );
query->setGimbalElevation( LCreal(getElevation()) );
// c) Send the query to the target
targets[i]->event(IR_QUERY, query);
// d) Dispose of the query
if (query->getRefCount() <= 1) {
// Recycle the query packet
query->clear();
lcLock(freeQueryLock);
if (freeQueryStack.isNotFull()) {
freeQueryStack.push(query);
}
else {
query->unref();
}
lcUnlock(freeQueryLock);
}
else {
// Store for future reference
lcLock(inUseQueryLock);
if (inUseQueryQueue.isNotFull()) {
inUseQueryQueue.put(query);
}
else {
// Just forget it
query->unref();
}
lcUnlock(inUseQueryLock);
}
}
else {
// When we couldn't get a free query packet
if (isMessageEnabled(MSG_WARNING)) {
std::cerr << "Iw Seeker: OUT OF Query messages!" << std::endl;
}
}
}
}
// Unref() the TDB
tdb0->unref();
}
void MergingIrSensor::mergeIrReturns()
{
int numRecords = storedMessagesQueue.entries();
if (numRecords > 0) {
//int* deleteArray = new int [numRecords];
//if (deleteArray == 0) {
// if (isMessageEnabled(MSG_ERROR)) {
// std::cerr << "Error: Allocation memory failure in IrSensor::mergeIrReturns" << std::endl;
// }
//}
//else {
//for (int i=0; i < numRecords; i++) {
// deleteArray[i] = 0;
//}
lcLock(storedMessagesLock);
// Traverse the stored message queue using peek (). Examine every
// message. Compare every stored message against every OTHER stored
// message. If the two are too close together, merge the two signals
// and mark the second message in the delete array.
// Proceed through the loop, ignoring all messages marked "deleted"
// in the delete array.
numRecords = storedMessagesQueue.entries();
if (isMessageEnabled(MSG_DEBUG)) {
std::cout << "IrSensor: numRecords returned " << numRecords << std::endl;
}
for (int i=0; i < numRecords; i++) {
//if (deleteArray[i] == 0) { // Do not bother processing those marked
// for deletion -- these have already been
// merged and must be ignored.
IrQueryMsg* currentMsg = storedMessagesQueue.peek0(i);
// Do not bother processing those marked
// for deletion -- these have already been
// merged and must be ignored.
if (currentMsg->getQueryMergeStatus()!= IrQueryMsg::MERGED_OUT) {
for (int j = i+1; j < numRecords; j++) {
IrQueryMsg* nextMsg = storedMessagesQueue.peek0(j);
LCreal azimuthDelta = currentMsg->getRelativeAzimuth() - nextMsg->getRelativeAzimuth();
LCreal elevationDelta = currentMsg->getRelativeElevation()
- nextMsg->getRelativeElevation();
if (azimuthDelta < 0)
azimuthDelta = -azimuthDelta;
if (elevationDelta < 0)
elevationDelta = -elevationDelta;
if ((azimuthDelta < azimuthBin) &&
(elevationDelta < elevationBin)) { // two signals are too close together
// for the sensor to distinguish between them;
// we will merge the two signals based
// on their weighted signal-to-noise.
LCreal currentRatio = 0.0;
LCreal nextRatio = 0.0;
// find current ratio.
if (isMessageEnabled(MSG_DEBUG)) {
std::cout << "IrSensor: merging target " << nextMsg->getTarget()->getName()->getString()
<< " into target " <<currentMsg->getTarget()->getName()->getString() << std::endl;
}
if (currentMsg->getSignalToNoiseRatio() >
currentMsg->getBackgroundNoiseRatio()) {
currentRatio = currentMsg->getSignalToNoiseRatio() +
currentMsg->getBackgroundNoiseRatio();
} else {
if (currentMsg->getSignalToNoiseRatio() < 0) {
currentRatio = -currentMsg->getSignalToNoiseRatio() -
currentMsg->getBackgroundNoiseRatio();
} else {
currentRatio = -currentMsg->getSignalToNoiseRatio() -
currentMsg->getBackgroundNoiseRatio();
} // signaltonoise < 0
} // if current signal > background
//now do the same thing for the next message.
if (nextMsg->getSignalToNoiseRatio() >
nextMsg->getBackgroundNoiseRatio()) {
nextRatio = nextMsg->getSignalToNoiseRatio() +
nextMsg->getBackgroundNoiseRatio();
} else {
if (nextMsg->getSignalToNoiseRatio() < 0) {
nextRatio = -nextMsg->getSignalToNoiseRatio() -
nextMsg->getBackgroundNoiseRatio();
} else {
nextRatio = -nextMsg->getSignalToNoiseRatio() -
nextMsg->getBackgroundNoiseRatio();
} // signaltonoise < 0
} // if next signal > background
// use ratios to find weights.
LCreal sumRatio = currentRatio + nextRatio;
const LCreal currentWeight = currentRatio / sumRatio;
const LCreal nextWeight = 1.0 - currentWeight;
//combine line-of-sight vector using weights
currentMsg->setLosVec((currentMsg->getLosVec() * currentWeight) +
(nextMsg->getLosVec() * nextWeight));
// combine position
currentMsg->setPosVec((currentMsg->getPosVec() * currentWeight) +
(nextMsg->getPosVec() * nextWeight));
// combine velocity
currentMsg->setVelocityVec((currentMsg->getVelocityVec() * currentWeight) +
(nextMsg->getVelocityVec() * nextWeight));
// combine acceleration
currentMsg->setAccelVec((currentMsg->getAccelVec() * currentWeight) +
(nextMsg->getAccelVec() * nextWeight));
// combine signal to noise ratios.
sumRatio = sumRatio - currentMsg->getBackgroundNoiseRatio();
if (sumRatio < 0)
sumRatio = -sumRatio;
currentMsg->setSignalToNoiseRatio(sumRatio);
//combine Azimuth and Elevation.
currentMsg->setAzimuthAoi((currentMsg->getAzimuthAoi() * currentWeight) +
nextMsg->getAzimuthAoi() * nextWeight);
currentMsg->setElevationAoi((currentMsg->getElevationAoi()* currentWeight) +
(nextMsg->getElevationAoi() * nextWeight));
currentMsg->setAngleAspect((currentMsg->getAngleAspect() * currentWeight) +
(nextMsg->getAngleAspect() * nextWeight));
currentMsg->setRelativeAzimuth((currentMsg->getRelativeAzimuth() * currentWeight) +
(nextMsg->getRelativeAzimuth() * nextWeight));
currentMsg->setRelativeElevation((currentMsg->getRelativeElevation() * currentWeight) +
(nextMsg->getRelativeElevation() * nextWeight));
// signal that this report has merged targets
currentMsg->setQueryMergeStatus(IrQueryMsg::MERGED);
nextMsg->setQueryMergeStatus(IrQueryMsg::MERGED_OUT);
//deleteArray[j] = 1; // now that we have merged this signal with the
// Ith signal, it must be deleted. It will not
// be passed on to the track manager.
//if (isMessageEnabled(MSG_INFO)) {
//std::cout << "IrSensor: End Merge" << std::endl;
//}
} // if we merge
} // end for j = i + 1;
} // End if delete Array
else { // debug - this target ws merged into another
int x=0;
x=x+1;
}
} // end for i = 0;
lcUnlock(storedMessagesLock);
//delete[] deleteArray;
//} // newArray is not null.
} // numRecords > 0
}
//------------------------------------------------------------------------------
// Open the data file
//------------------------------------------------------------------------------
bool FileReader::openFile()
{
// When we're already open, just return
if (isOpen()) return true;
// local flags (default is success)
bool tOpened = true;
bool tFailed = false;
// Need a file name
if (filename == nullptr || filename->len() == 0) {
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "FileReader::openFile(): Unable to open data file: no file name" << std::endl;
}
tOpened = false;
tFailed = true;
}
else {
//---
// Allocate space for the full file name
//---
size_t nameLength = 0;
if (pathname != nullptr) {
nameLength += pathname->len(); // add the length of the path name
nameLength += 1; // add a character for the slash
}
nameLength += filename->len(); // add the length of the file name
nameLength += 1; // Add one for the null(0) at the end of the string
char* fullname = new char[nameLength];
fullname[0] = '\0';
//---
// Create the (initial) full file name
//---
if (pathname != nullptr && pathname->len() > 0) {
lcStrcat(fullname, nameLength ,*pathname);
lcStrcat(fullname, nameLength, "/");
}
lcStrcat(fullname,nameLength,*filename);
//---
// Make sure that it exists
//---
bool validName = doesFileExist(fullname);
//---
// When we have a valid file name ...
//---
if ( validName ) {
//---
// Make sure we have an input stream
//---
if (sin == nullptr) sin = new std::ifstream();
//---
// Open the file (binary input mode)
//---
sin->open(fullname, std::ios_base::in | std::ios_base::binary );
if (isMessageEnabled(MSG_INFO)) {
std::cout << "FileReader::openFile() Opening data file = " << fullname << std::endl;
}
if (sin->fail()) {
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "FileReader::openFile(): Failed to open data file: " << fullname << std::endl;
}
tOpened = false;
tFailed = true;
}
}
delete[] fullname;
}
fileOpened = tOpened;
fileFailed = tFailed;
return fileOpened;
}
//------------------------------------------------------------------------------
// Read basic information about this cell from the file headers
//------------------------------------------------------------------------------
bool DtedFile::readDtedHeaders(std::istream& in)
{
// Read in the User Header Label (UHL) record
dtedUhlRecord uhl;
in.read(reinterpret_cast<char*>(&uhl), sizeof(uhl));
if (in.fail() || in.gcount() < sizeof(uhl))
{
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "DtedFile::readDtedHeaders: error reading UHL record." << std::endl;
}
return false;
}
if (std::strncmp(uhl.recognition_sentinel, UHL_RECOGNITION_SENTINEL, sizeof(uhl.recognition_sentinel)) != 0)
{
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "DtedFile::readDtedHeaders: invalid recognition sentinel in UHL record." << std::endl;
}
return false;
}
if (uhl.fixed_by_standard[0] != UHL_FIXED_BY_STANDARD_BYTE)
{
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "DtedFile::readDtedHeaders: invalid fixed_by_standard byte in UHL record." << std::endl;
}
return false;
}
// Read in the Data Set Identification (DSI) record
dtedDsiRecord dsi;
in.read(reinterpret_cast<char*>(&dsi), sizeof(dsi));
if (in.fail() || in.gcount() < sizeof(dsi))
{
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "DtedFile::readDtedHeaders: error reading DSI record." << std::endl;
}
return false;
}
// Read in the Accuracy Description (ACC) record
dtedAccRecord acc;
in.read(reinterpret_cast<char*>(&acc), sizeof(acc));
if (in.fail() || in.gcount() < sizeof(acc))
{
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "DtedFile::readDtedHeaders: error reading ACC record." << std::endl;
}
return false;
}
// Extract the latitude and longitude of cell's SW corner.
// DTED cells are always 1 degree by 1 degree,
// and always start on an integer degree.
int swcLatitude(0);
int swcLongitude(0);
std::sscanf(uhl.origin_latitude, "%3d", &swcLatitude);
std::sscanf(uhl.origin_longitude, "%3d", &swcLongitude);
if (uhl.origin_latitude[7] == 'S')
swcLatitude = -1*swcLatitude;
if (uhl.origin_longitude[7] == 'W')
swcLongitude = -1*swcLongitude;
setLatitudeSW( swcLatitude );
setLongitudeSW( swcLongitude );
setLatitudeNE( swcLatitude + 1 );
setLongitudeNE( swcLongitude + 1 );
// Extract the data intervals for latitude and longitude
static const double TENTHS_OF_SECONDS_PER_DEGREE = 36000.0;
int latIncr(0);
int lonIncr(0);
std::sscanf(uhl.data_interval_latitude, "%4d", &latIncr);
std::sscanf(uhl.data_interval_longitude, "%4d", &lonIncr);
latSpacing = latIncr / TENTHS_OF_SECONDS_PER_DEGREE;
lonSpacing = lonIncr / TENTHS_OF_SECONDS_PER_DEGREE;
// Extract the number of latitude and longitude lines
unsigned int num_lat(0);
unsigned int num_lon(0);
std::sscanf(uhl.number_latitude_lines, "%4u", &num_lat);
std::sscanf(uhl.number_longitude_lines, "%4u", &num_lon);
nptlat = num_lat;
nptlong = num_lon;
return true;
}
//------------------------------------------------------------------------------
// weaponGuidance() -- default guidance; using Robot Aircraft (RAC) guidance
//------------------------------------------------------------------------------
void Missile::weaponGuidance(const LCreal dt)
{
// ---
// Control velocity: During burn time, accel to max velocity,
// after burn time, deaccelerate to min velocity.
// ---
if (isEngineBurnEnabled()) cmdVelocity = vpMax;
else cmdVelocity = vpMin;
// ---
// If the target's already dead,
// then don't go away mad, just go away.
// ---
const Player* tgt = getTargetPlayer();
const Track* trk = getTargetTrack();
if (trk != 0) tgt = trk->getTarget();
if (tgt != 0 && !tgt->isActive()) return;
osg::Vec3 los; // Target Line of Sight
osg::Vec3 vel; // Target velocity
// ---
// Basic guidance
// ---
{
// ---
// Get position and velocity vectors from the target/track
// ---
osg::Vec3 posx;
calculateVectors(tgt, trk, &los, &vel, &posx);
// compute range to target
LCreal trng0 = trng;
trng = los.length();
// compute range rate,
LCreal trdot0 = trdot;
if (dt > 0)
trdot = (trng - trng0)/dt;
else
trdot = 0;
// Target total velocit
LCreal totalVel = vel.length();
// compute target velocity parallel to LOS,
LCreal vtplos = (los * vel/trng);
// ---
// guidance - fly to intercept point
// ---
// if we have guidance ...
if ( isGuidanceEnabled() && trng > 0) {
// get missile velocity (must be faster than target),
LCreal v = vpMax;
if (v < totalVel) v = totalVel + 1;
// compute target velocity normal to LOS squared,
LCreal tgtVp = totalVel;
LCreal vtnlos2 = tgtVp*tgtVp - vtplos*vtplos;
// and compute missile velocity parallex to LOS.
LCreal vmplos = lcSqrt( v*v - vtnlos2 );
// Now, use both velocities parallel to LOS to compute
// closure rate.
LCreal vclos = vmplos - vtplos;
// Use closure rate and range to compute time to intercept.
LCreal dt1 = 0;
if (vclos > 0) dt1 = trng/vclos;
// Use time to intercept to extrapolate target position.
osg::Vec3 p1 = (los + (vel * dt1));
// Compute missile commanded heading and
cmdHeading = lcAtan2(p1.y(),p1.x());
// commanded pitch.
LCreal grng = lcSqrt(p1.x()*p1.x() + p1.y()*p1.y());
cmdPitch = -lcAtan2(p1.z(),grng);
}
}
// ---
// fuzing logic (let's see if we've scored a hit)
// (compute range at closest point and compare to max burst radius)
// (use target truth data)
// ---
{
// ---
// Get position and velocity vectors from the target (truth)
// (or default to the values from above)
// ---
if (tgt != 0) {
calculateVectors(tgt, 0, &los, &vel, 0);
}
// compute range to target
LCreal trng0 = trngT;
trngT = los.length();
// compute range rate,
LCreal trdot0 = trdotT;
if (dt > 0)
trdotT = (trngT - trng0)/dt;
else
trdotT = 0;
// when we've just passed the target ...
if (trdotT > 0 && trdot0 < 0 && !isDummy() && getTOF() > 2.0f) {
bool missed = true; // assume the worst
// compute relative velocity vector.
osg::Vec3 velRel = (vel - getVelocity());
// compute missile velocity squared,
LCreal vm2 = velRel.length2();
if (vm2 > 0) {
// relative range (dot) relative velocity
LCreal rdv = los * velRel;
// interpolate back to closest point
LCreal ndt = -rdv/vm2;
osg::Vec3 p0 = los + (velRel*ndt);
// range squared at closest point
LCreal r2 = p0.length2();
// compare to burst radius squared
if (r2 <= (getMaxBurstRng()*getMaxBurstRng()) ) {
// We've detonated
missed = false;
setMode(DETONATED);
setDetonationResults( DETONATE_ENTITY_IMPACT );
// compute location of the detonation relative to the target
osg::Vec3 p0n = -p0;
if (tgt != 0) p0n = tgt->getRotMat() * p0n;
setDetonationLocation(p0n);
// Did we hit anyone?
checkDetonationEffect();
// Log the event
LCreal detRange = getDetonationRange();
if (isMessageEnabled(MSG_INFO)) {
std::cout << "DETONATE_ENTITY_IMPACT rng = " << detRange << std::endl;
}
if (getAnyEventLogger() != 0) {
TabLogger::TabLogEvent* evt = new TabLogger::LogWeaponActivity(2, getLaunchVehicle(), this, getTargetPlayer(), DETONATE_ENTITY_IMPACT, detRange); // type 2 for "detonate"
getAnyEventLogger()->log(evt);
evt->unref();
}
}
}
// Did we miss the target?
if (missed) {
// We've detonated ...
setMode(DETONATED);
setDetonationResults( DETONATE_DETONATION );
// because we've just missed the target
setTargetPlayer(0,false);
setTargetTrack(0,false);
// Log the event
LCreal detRange = trngT;
if (isMessageEnabled(MSG_INFO)) {
std::cout << "DETONATE_OTHER rng = " << detRange << std::endl;
}
if (getAnyEventLogger() != 0) {
TabLogger::TabLogEvent* evt = new TabLogger::LogWeaponActivity(2, getLaunchVehicle(), this, getTargetPlayer(), DETONATE_DETONATION, getDetonationRange()); // type 2 for "detonate"
getAnyEventLogger()->log(evt);
evt->unref();
}
}
}
}
}
// -------------------------------------------------------------
// bindSocket() -- bind the socket to an address, and configure
// the send and receive buffers.
// -------------------------------------------------------------
bool PosixHandler::bindSocket()
{
if (socketNum == INVALID_SOCKET) return false;
// ---
// Set the reuse socket attribute
// ---
{
#if defined(WIN32)
BOOL optval = getSharedFlag();
socklen_t optlen = sizeof(optval);
if (::setsockopt(socketNum, SOL_SOCKET, SO_REUSEADDR, (const char*) &optval, optlen) == SOCKET_ERROR) {
#else
int optval = getSharedFlag();
socklen_t optlen = sizeof(optval);
if (::setsockopt(socketNum, SOL_SOCKET, SO_REUSEADDR, &optval, optlen) == SOCKET_ERROR) {
#endif
std::perror("PosixHandler::bindSocket(): error setsockopt(SO_REUSEADDR)\n");
return false;
}
}
return true;
}
//------------------------------------------------------------------------------
// Set the output buffer size
//------------------------------------------------------------------------------
bool PosixHandler::setSendBuffSize()
{
if (socketNum == INVALID_SOCKET) return false;
const unsigned int optval = sendBuffSizeKb * 1024;
socklen_t optlen = sizeof(optval);
#if defined(WIN32)
if (::setsockopt(socketNum, SOL_SOCKET, SO_SNDBUF, reinterpret_cast<const char*>(&optval), optlen) == SOCKET_ERROR) {
#else
if (::setsockopt(socketNum, SOL_SOCKET, SO_SNDBUF, reinterpret_cast<const void*>(&optval), optlen) == SOCKET_ERROR) {
#endif
std::perror("PosixHandler::setSendBuffSize(): error setting the send buffer size\n");
return false;
}
return true;
}
//------------------------------------------------------------------------------
// Sets the input buffer size
//------------------------------------------------------------------------------
bool PosixHandler::setRecvBuffSize()
{
if (socketNum == INVALID_SOCKET) return false;
const unsigned int optval = recvBuffSizeKb * 1024;
socklen_t optlen = sizeof (optval);
#if defined(WIN32)
if (::setsockopt(socketNum, SOL_SOCKET, SO_RCVBUF, reinterpret_cast<const char*>(&optval), optlen) == SOCKET_ERROR) {
#else
if (::setsockopt(socketNum, SOL_SOCKET, SO_RCVBUF, reinterpret_cast<const void*>(&optval), optlen) == SOCKET_ERROR) {
#endif
std::perror("PosixHandler::setRecvBuffSize(): error setting the receive buffer size\n");
return false;
}
return true;
}
// -------------------------------------------------------------
// setBlocked() -- Sets blocked I/O mode
// -------------------------------------------------------------
bool PosixHandler::setBlocked()
{
if (socketNum == NET_INVALID_SOCKET) return false;
// Set the socket 'sock' to Blocking. Wait I/O.
#if defined(WIN32)
unsigned long zz = false;
if (::ioctlsocket(socketNum, FIONBIO, &zz) == SOCKET_ERROR) {
std::perror("PosixHandler::setBlocked()");
return false;
}
#else
const int zz = 0;
if (::ioctl(socketNum, FIONBIO, &zz) == SOCKET_ERROR) {
std::perror("PosixHandler::setBlocked()");
return false;
}
#endif
return true;
}
// -------------------------------------------------------------
// setNoWait() -- Sets no wait (non-blocking) I/O mode
// -------------------------------------------------------------
bool PosixHandler::setNoWait()
{
if (socketNum == NET_INVALID_SOCKET) return false;
// Set the socket 'sock' to Non-Blocking. Nowait I/O.
#if defined(WIN32)
unsigned long zz = true;
if (::ioctlsocket(socketNum, FIONBIO, &zz ) == SOCKET_ERROR) {
std::perror("PosixHandler::setNoWait()");
return false;
}
#else
const int zz = 1;
if (::ioctl(socketNum, FIONBIO, &zz ) == SOCKET_ERROR) {
std::perror("PosixHandler::setNoWait()");
return false;
}
#endif
return true;
}
// -------------------------------------------------------------
// Returns true if the network handler has been initialized
// -------------------------------------------------------------
bool PosixHandler::isConnected() const
{
return initialized;
}
// -------------------------------------------------------------
// Close (un-initialize) this network
// -------------------------------------------------------------
bool PosixHandler::closeConnection()
{
initialized = false;
return true;
}
// -------------------------------------------------------------
// sendData() -- Send data
// -------------------------------------------------------------
bool PosixHandler::sendData(const char* const packet, const int size)
{
if (socketNum == INVALID_SOCKET) return false;
// Send the data
struct sockaddr_in addr; // Working address structure
bzero(&addr, sizeof(addr));
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = netAddr;
addr.sin_port = htons(port);
socklen_t addrlen = sizeof(addr);
int result = ::sendto(socketNum, packet, size, 0, reinterpret_cast<const struct sockaddr*>(&addr), addrlen);
if (result == SOCKET_ERROR) {
#if defined(WIN32)
int err = ::WSAGetLastError();
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "PosixHandler::sendData(): sendto error: " << err << " hex=0x" << std::hex << err << std::dec << std::endl;
}
#else
std::perror("PosixHandler::sendData(): sendto error msg");
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "PosixHandler::sendData(): sendto error result: " << result << std::endl;
}
#endif
return false;
}
return true;
}
// -------------------------------------------------------------
// recvData() -- Receive data and possible ignore our own
// local port messages.
// -------------------------------------------------------------
unsigned int PosixHandler::recvData(char* const packet, const int maxSize)
{
unsigned int n = 0;
if (socketNum == INVALID_SOCKET) return 0;
fromAddr1 = INADDR_NONE;
fromPort1 = 0;
bool tryAgain = true;
while (tryAgain) {
tryAgain = false;
// Try to receive the data
struct sockaddr_in raddr; // IP address
socklen_t addrlen = sizeof(raddr);
int result = ::recvfrom(socketNum, packet, maxSize, 0, reinterpret_cast<struct sockaddr*>(&raddr), &addrlen);
if (result > 0 && ignoreSourcePort != 0) {
// Ok we have one; make sure it's not one we should ignore
uint16_t rport = ntohs(raddr.sin_port);
if (rport == ignoreSourcePort) {
tryAgain = true;
}
}
// set number of bytes received
if (result > 0 && !tryAgain) {
n = result;
fromAddr1 = raddr.sin_addr.s_addr;
fromPort1 = ntohs(raddr.sin_port);
}
}
return n;
}
//------------------------------------------------------------------------------
// Set functions
//------------------------------------------------------------------------------
// Set the shared flag
void PosixHandler::setSharedFlag(const bool b)
{
sharedFlg = b;
}
// Set port number
bool PosixHandler::setPort(const uint16_t n1)
{
port = n1;
return true;
}
//------------------------------------------------------------------------------
// Add the NTM to our sublist of nodes.
//------------------------------------------------------------------------------
bool NtmInputNode::add2OurLists(simulation::Ntm* const ntm)
{
bool ok = false;
// Make sure we have the correct kind of NTM ...
rprfom::Ntm* disNtm = dynamic_cast<rprfom::Ntm*>( ntm );
if (disNtm != nullptr) {
// Make sure that the NTM's code for this level matches our code
unsigned int currLevelCode = 0;
unsigned int nextLevelCode = 0;
switch (level) {
case ROOT_LVL : {
currLevelCode = 0;
nextLevelCode = disNtm->getEntityKind();
break;
}
case KIND_LVL : {
currLevelCode = disNtm->getEntityKind();
nextLevelCode = disNtm->getEntityDomain();
break;
}
case DOMAIN_LVL : {
currLevelCode = disNtm->getEntityDomain();
nextLevelCode = disNtm->getEntityCountry();
break;
}
case COUNTRYCODE_LVL : {
currLevelCode = disNtm->getEntityCountry();
nextLevelCode = disNtm->getEntityCategory();
break;
}
case CATEGORY_LVL : {
currLevelCode = disNtm->getEntityCategory();
nextLevelCode = disNtm->getEntitySubcategory();
break;
}
case SUBCATEGORY_LVL : {
currLevelCode = disNtm->getEntitySubcategory();
nextLevelCode = disNtm->getEntitySpecific();
break;
}
case SPECIFIC_LVL : {
currLevelCode = disNtm->getEntitySpecific();
nextLevelCode = disNtm->getEntityExtra();
break;
}
case EXTRA_LVL : {
currLevelCode = disNtm->getEntityExtra();
nextLevelCode = 0;
break;
}
}
// Does our code match the NIB's entity type code for this level?
// And the 'root' node always matches.
bool match = (code == currLevelCode) || (level == ROOT_LVL);
if (match) {
bool err = false;
// Case #1; if we're at the 'category' level or above, and all remaining codes are
// zero, then this becomes a wild card terminal node.
{
bool wild = (level >= CATEGORY_LVL);
if (wild && level < EXTRA_LVL) wild = (disNtm->getEntityExtra() == 0);
if (wild && level < SPECIFIC_LVL) wild = (disNtm->getEntitySpecific() == 0);
if (wild && level < SUBCATEGORY_LVL) wild = (disNtm->getEntitySubcategory() == 0);
if (wild) {
// wild card terminal node
if (ourNtm == nullptr) {
ourNtm = disNtm;
ourNtm->ref();
ok = true;
}
else if (isMessageEnabled(MSG_WARNING)) {
std::cerr << "Warning: duplicate incoming NTM(";
std::cerr << int(disNtm->getEntityKind()) << ",";
std::cerr << int(disNtm->getEntityDomain()) << ",";
std::cerr << int(disNtm->getEntityCountry()) << ",";
std::cerr << int(disNtm->getEntityCategory()) << ",";
std::cerr << int(disNtm->getEntitySubcategory()) << ",";
std::cerr << int(disNtm->getEntitySpecific()) << ",";
std::cerr << int(disNtm->getEntityExtra()) << ")";
std::cerr << ", second ignored" << std::endl;
err = true;
}
}
}
// Case #2; if we're at the 'specific' level, then create a terminal node
// for the Ntm. The wild card case was handle in case #1.
if (!ok && !err && level == SPECIFIC_LVL) {
// make sure the terminal node doesn't already exist.
bool alreadyExists = false;
const base::List::Item* item = subnodeList->getFirstItem();
while (item != nullptr && !alreadyExists) {
const NtmInputNode* subnode = static_cast<const NtmInputNode*>(item->getValue());
alreadyExists = (nextLevelCode == subnode->code);
item = item->getNext();
}
if (!alreadyExists) {
NtmInputNode* newNode = new NtmInputNode( (level+1), nextLevelCode, disNtm );
subnodeList->put(newNode);
newNode->unref(); // ref()'d when put into the subnode list
ok = true;
}
else if (isMessageEnabled(MSG_WARNING)) {
std::cerr << "Warning: duplicate incoming NTM(";
std::cerr << int(disNtm->getEntityKind()) << ",";
std::cerr << int(disNtm->getEntityDomain()) << ",";
std::cerr << int(disNtm->getEntityCountry()) << ",";
std::cerr << int(disNtm->getEntityCategory()) << ",";
std::cerr << int(disNtm->getEntitySubcategory()) << ",";
std::cerr << int(disNtm->getEntitySpecific()) << ",";
std::cerr << int(disNtm->getEntityExtra()) << ")";
std::cerr << ", second ignored" << std::endl;
err = true;
}
}
// Case #3; if we're at a level less than the 'specific' level, so try
// to add the NTM to one of our existing subnodes.
if (!ok && !err && level < SPECIFIC_LVL) {
const base::List::Item* item = subnodeList->getFirstItem();
while (item != nullptr && !ok) {
NtmInputNode* subnode = const_cast<NtmInputNode*>(static_cast<const NtmInputNode*>(item->getValue()));
if (nextLevelCode == subnode->code) {
ok = subnode->add2OurLists(disNtm);
}
item = item->getNext();
}
}
// Case #4; We didn't create a terminal node, and the NTM was added to
// one of our existing subnodes, then create a new subnode for it.
if (!ok && !err) {
// Create a new node and add the NTM
NtmInputNode* newNode = new NtmInputNode( (level+1), nextLevelCode );
subnodeList->put(newNode);
ok = newNode->add2OurLists(disNtm);
newNode->unref(); // ref()'d when put into the subnode list
}
}
}
return ok;
}
//------------------------------------------------------------------------------
// Halo Font functions
// FtglHaloFont::outputText() -- FtglHaloFont output routines
//------------------------------------------------------------------------------
void FtglHaloFont::outputText(const double x, const double y, const char* txt, const int n, const bool vf, const bool rf)
{
GLfloat ocolor[4];
glGetFloatv(GL_CURRENT_COLOR, ocolor);
// Make sure we have characters to print
if (n <= 0) return;
// Make sure we have a loaded font
if (isNotLoaded()) {
loadFont();
if (isNotLoaded()) throw new ExpInvalidFont();
}
// Prepare the output text
char cbuf[MSG_BUF_LEN];
int nn = xferChars(cbuf,MSG_BUF_LEN,txt,n);
if (nn <= 0) return;
if (haloColor != 0) Graphic::lcColor3(haloColor->red(), haloColor->green(), haloColor->blue());
// default to black if we have no color specified
else glColor3f(0,0,0);
// now render the outline font over it!
if (!rf) {
if (outline != 0) {
glPushMatrix();
glTranslated(x, y, -0.01f);
// get our current linewidth
GLfloat lw = 0;
glGetFloatv(GL_LINE_WIDTH, &lw);
glLineWidth(linewidth);
glScalef(GLfloat(getFontWidth()), GLfloat(getFontHeight()), 1.0f);
if (vf) {
// Vertical text
GLdouble dy = getLineSpacing();
if (getFontHeight() != 0.0) dy = getLineSpacing() / getFontHeight();
char cc[2];
cc[1] = '\0';
for (int i = 0; i < nn; i++) {
cc[0] = cbuf[i];
outline->Render(cc);
glTranslated(0.0, -dy, 0.0);
}
}
else {
// Normal text
outline->Render(cbuf);
}
glLineWidth(lw);
glPopMatrix();
}
else {
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "FtglHaloFont::outputText() - no outline font available" << std::endl;
}
}
}
glColor4fv(ocolor);
// output the text
FTGLPolygonFont* ftgl1 = (FTGLPolygonFont*)FTGL();
if (ftgl1 != 0) {
glPushMatrix();
glTranslated(x, y, 0.0);
glScalef(GLfloat(getFontWidth()), GLfloat(getFontHeight()), 1.0f);
if (vf) {
// Vertical text
GLdouble dy = getLineSpacing();
if (getFontHeight() != 0.0) dy = getLineSpacing() / getFontHeight();
char cc[2];
cc[1] = '\0';
for (int i = 0; i < nn; i++) {
cc[0] = cbuf[i];
ftgl1->Render(cc);
glTranslated(0.0, -dy, 0.0);
}
}
else {
// Normal text
ftgl1->Render(cbuf);
}
glPopMatrix();
}
else {
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "FtglPolygonFont::outputText() - no Polygon font available" << std::endl;
}
}
}
//------------------------------------------------------------------------------
// process() --
//------------------------------------------------------------------------------
void Sar::process(const LCreal dt)
{
BaseClass::process(dt);
// Imaging in porgress?
if (timer > 0) {
// ---
// Point the beam
// ---
Antenna* ant = getAntenna();
if (ant != 0) {
Simulation* s = getSimulation();
double refLat = s->getRefLatitude();
double refLon = s->getRefLongitude();
osg::Vec3 pos;
Basic::Nav::convertLL2PosVec(
refLat, refLon, // Ref point (at sea level)
getStarePointLatitude(), getStarePointLongitude(), getStarePointElevation(),
&pos); // x,y,z NED
// Platform (ownship) coord and then body
osg::Vec3 posP = pos - getOwnship()->getPosition();
osg::Vec3 posB = getOwnship()->getRotMat() * posP;
// Convert to az/el
LCreal tgt_az = 0.0; // Angle (degs)
LCreal tgt_el = 0.0; // Angle (degs)
xyz2AzEl(posB, &tgt_az, &tgt_el);
// Command to that position
LCreal az = tgt_az * (LCreal)Basic::Angle::D2RCC;
LCreal el = tgt_el * (LCreal)Basic::Angle::D2RCC;
ant->setRefAzimuth(az);
ant->setRefElevation(el);
ant->setScanMode(Antenna::CONICAL_SCAN);
}
// ---
// Process timer
// ---
LCreal ttimer = timer - dt;
if (ttimer <= 0) {
// ### test -- Generate a test image ###
Image* p = new Image();
p->testImage(width,height);
p->setImageId(getNextId());
p->setLatitude(getStarePointLatitude());
p->setLongitude(getStarePointLongitude());
p->setElevation(getStarePointElevation());
p->setOrientation(0);
if (isMessageEnabled(MSG_INFO)) {
std::cout << "Sar:: Generating test image: resolution: " << getResolution() << std::endl;
}
if (getResolution() > 0) p->setResolution( getResolution() );
else p->setResolution( 3.0f * Basic::Distance::FT2M );
Basic::Pair* pp = new Basic::Pair("image", p);
addImage(pp);
// ### TEST TEST
// Just finished!
ttimer = 0;
setTransmitterEnableFlag(false);
}
timer = ttimer;
}
BaseClass::updateData(dt);
}
//------------------------------------------------------------------------------
// Serialize and write a DataRecord to a file
//------------------------------------------------------------------------------
void FileWriter::processRecordImp(const DataRecordHandle* const handle)
{
bool thisIsEodMsg = false;
// ---
// Open the file, if it hasn't been already ...
// ---
if ( !fileOpened && !fileFailed ) openFile();
// ---
// When the file is open and ready ...
// --- serialize and write the data record.
// ---
if ( fileOpened ) {
// The DataRecord to be sent
const Pb::DataRecord* dataRecord = handle->getRecord();
// Serialize the DataRecord
std::string wireFormat;
bool ok = dataRecord->SerializeToString(&wireFormat);
// Write the serialized DataRecord with its length to the file
if (ok) {
unsigned int n = wireFormat.length();
// Convert size to an integer string
char nbuff[8];
std::sprintf(nbuff, "%04d", n);
// Convert the leading zeros to spaces
if (nbuff[0] == '0') {
nbuff[0] = ' ';
if (nbuff[1] == '0') {
nbuff[1] = ' ';
if (nbuff[2] == '0') {
nbuff[2] = ' ';
}
}
}
// Write the size of the serialized DataRecord as an ascii string
sout->write(nbuff, 4);
// Write the serialized DataRecord
sout->write( wireFormat.c_str(), n );
}
else if (isMessageEnabled(MSG_ERROR | MSG_WARNING)) {
// If we had an error serializing the DataRecord
std::cerr << "FileWriter::processRecordImp() -- SerializeToString() error" << std::endl;
}
// Check for END_OF_DATA message
thisIsEodMsg = (dataRecord->id() == REID_END_OF_DATA);
}
// ---
// Close the file at END_OF_DATA message
// ---
if (!eodFlag && thisIsEodMsg) {
eodFlag = true;
closeFile();
}
}
//------------------------------------------------------------------------------
// Read elevation data from DTED file
//------------------------------------------------------------------------------
bool DtedFile::readDtedData(std::istream& in)
{
if (nptlat < 1 || nptlong < 1)
{
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "DtedFile::readDtedData: DTED headers indicate an empty file." << std::endl;
}
return false;
}
// Allocate the elevation array
columns = new short*[nptlong];
for(unsigned int i=0; i<nptlong; i++)
{
columns[i] = new short[nptlat];
}
// Read the elevation array.
for(unsigned int lon=0; lon<nptlong; lon++)
{
unsigned long checksum = 0;
// read record header
dtedColumnHeader head;
in.read(reinterpret_cast<char*>(&head), sizeof(head));
if (in.fail() || in.gcount() < sizeof(head))
{
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "DtedFile::readDtedData: error reading column header." << std::endl;
}
return false;
}
for(unsigned int i=0; i<sizeof(head); i++)
checksum += (reinterpret_cast<unsigned char*>(&head))[i];
if (head.recognition_sentinel[0] != DATA_RECOGNITION_SENTINEL)
{
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "DtedFile::readDtedData: record contains invalid recognition sentinel." << std::endl;
}
return false;
}
// Read elevation values for record
double minElev0 = 99999.0;
double maxElev0 = 0.0;
for(unsigned int lat=0; lat<nptlat; lat++)
{
unsigned char values[2];
in.read(reinterpret_cast<char*>(values), sizeof(values));
if (in.fail() || in.gcount() < sizeof(values))
{
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "DtedFile::readDtedData: error reading data value." << std::endl;
}
return false;
}
checksum += values[0] + values[1];
short height = readValue(values[0], values[1]);
columns[lon][lat] = height;
// check if this is the new min or max elevation
if (height < minElev0) minElev0 = height;
if (height > maxElev0) maxElev0 = height;
}
setMinElevation(minElev0);
setMaxElevation(maxElev0);
// Read data record footer and verify checksum
dtedColumnFooter foot;
in.read(reinterpret_cast<char*>(&foot), sizeof(foot));
if (in.fail() || in.gcount() < sizeof(foot))
{
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "DtedFile::readDtedData: error reading column footer." << std::endl;
}
return false;
}
if (isVerifyChecksum()) {
unsigned long file_cksum = readValue(foot.checksum[0], foot.checksum[1], foot.checksum[2], foot.checksum[3]);
if (file_cksum != checksum)
{
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "DtedFile::readDtedData: bad checksum in data record." << std::endl;
}
return false;
}
}
}
return true;
}
//------------------------------------------------------------------------------
// Open the data file
//------------------------------------------------------------------------------
bool PrintHandler::openFile()
{
// When we're already open, just return
if (isOpen()) return true;
// If we don't have a file name then we're using the standard output
if (filename == 0 || filename->len() == 0) return true;
// clear the old 'full' file name
setFullFilename(0);
// local flags (default is success)
bool tOpened = true;
bool tFailed = false;
//---
// Allocate space for the full file name
//---
size_t nameLength = 0;
if (pathname != 0) {
nameLength += pathname->len(); // add the length of the path name
nameLength += 1; // add a character for the slash
}
nameLength += filename->len(); // add the length of the file name
nameLength += 4; // add characters for possible version number, "_V99"
nameLength += 1; // Add one for the null(0) at the end of the string
char* fullname = new char[nameLength];
fullname[0] = '\0';
//---
// Create the (initial) full file name
//---
if (pathname != 0 && pathname->len() > 0) {
lcStrcat(fullname, nameLength ,*pathname);
lcStrcat(fullname, nameLength, "/");
}
lcStrcat(fullname,nameLength,*filename);
//---
// Make sure that it doesn't already exist (we don't want to over write good data).
//---
bool validName = !doesFileExist(fullname);
if ( !validName ) {
// If the file already exists, try appending a version number "v99" ..
char* origname = new char[nameLength];
lcStrcpy(origname, nameLength, fullname);
validName = false;
for (unsigned int i = 1; i <= 99 && !validName; i++) {
std::sprintf(fullname, "%s_v%02d", origname, i);
validName = !doesFileExist(fullname);
}
if ( !validName ) {
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "PrintHandler::openFile(): All version of the data file already exists: " << origname << std::endl;
}
tOpened = false;
tFailed = true;
}
delete[] origname;
}
//---
// When we have a valid file name ...
//---
if ( validName ) {
// The file name with the path and version number
setFullFilename(fullname);
//---
// Make sure we have an output stream
//---
if (sout == 0) sout = new std::ofstream();
//---
// Open the file
//---
sout->open(fullname, std::ios_base::out);
if (isMessageEnabled(MSG_INFO)) {
std::cout << "PrintHandler::openFile() Opening data file = " << fullname << std::endl;
}
if (sout->fail()) {
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "PrintHandler::openFile(): Failed to open data file: " << fullname << std::endl;
}
tOpened = false;
tFailed = true;
}
}
delete[] fullname;
fileOpened = tOpened;
fileFailed = tFailed;
return fileOpened;
}
//-----------------------------------------------------------------------------
// createSubWindow() -- create the screen for a subwindow
//-----------------------------------------------------------------------------
int GlutDisplay::createSubWindow(const int mainId)
{
GLint vpX(0), vpY(0); // our initial viewport position
GLsizei vpWidth(0), vpHeight(0); // our initial viewport size
getViewport(&vpX, &vpY, &vpWidth, &vpHeight);
// Must have a width and height
winId = -1;
if (vpWidth == 0 || vpHeight == 0) return winId;
mainWinId = mainId;
unsigned int wmode = GLUT_DOUBLE | GLUT_RGB | GLUT_ALPHA;
if (getClearDepth() >= 0.0f) { wmode = wmode | GLUT_DEPTH; }
if (accumBuff) { wmode = wmode | GLUT_ACCUM; }
if (stencilBuff) { wmode = wmode | GLUT_STENCIL; }
glutInitDisplayMode( wmode );
winId = glutCreateSubWindow (mainWinId, vpX, vpY, vpWidth, vpHeight);
if (winId > 0) {
if (isMessageEnabled(MSG_INFO)) {
std::cout << "GlutDisplay::createSubWindow() name = " << getName() << ", winId = " << winId << std::endl;
}
// compute our sub-display to main display ratios
const GlutDisplay* pMainWin = findRegisteredGlutDisplay(mainWinId);
if (pMainWin != nullptr) {
GLint mainWinX = 0, mainWinY = 0;
GLsizei mainWinWidth = 0, mainWinHeight = 0;
pMainWin->getViewport(&mainWinX, &mainWinY, &mainWinWidth, &mainWinHeight);
const double widthRatio = static_cast<double>(vpWidth) / static_cast<double>(mainWinWidth);
const double heightRatio = static_cast<double>(vpHeight) / static_cast<double>(mainWinHeight);
const double xRatio = static_cast<double>(vpX) / static_cast<double>(mainWinWidth);
const double yRatio = static_cast<double>(vpY) / static_cast<double>(mainWinHeight);
swPosition.set(xRatio, yRatio);
swSize.set(widthRatio, heightRatio);
}
glutDisplayFunc(drawFuncCB);
glutReshapeFunc(reshapeItCB);
glutMouseFunc(mouseFuncCB);
glutKeyboardFunc(keyboardFuncCB);
glutSpecialFunc(specialFuncCB);
glutPassiveMotionFunc(passiveMotionFuncCB);
glutMotionFunc(motionFuncCB);
glutEntryFunc(entryFuncCB);
registerGlutDisplay(winId, this);
glutSetWindow(winId);
configure();
loadTextures();
// Create sub windows (if any)
if (subDisplays() != nullptr) {
Basic::List::Item* item = subDisplays()->getFirstItem();
while (item != nullptr) {
Basic::Pair* pair = dynamic_cast<Basic::Pair*>(item->getValue());
if (pair != nullptr) {
GlutDisplay* dobj = dynamic_cast<GlutDisplay*>( pair->object() );
if (dobj != nullptr) dobj->createSubWindow(winId);
}
item = item->getNext();
}
}
}
return winId;
}
// Load the font for one character
GLubyte* BitmapFont::loadTypeFace(const GLint index, const GLenum reverse)
{
// If no font to load, return
if (fontMap[index] == 0)
return 0;
// Create the font file name
const size_t FONTPATHNAME_LENGTH = 256;
char fontPathname[FONTPATHNAME_LENGTH];
if (fontDirectory() != 0)
lcStrcpy(fontPathname, FONTPATHNAME_LENGTH, fontDirectory());
else
lcStrcpy(fontPathname, FONTPATHNAME_LENGTH, "./");
lcStrcat(fontPathname, FONTPATHNAME_LENGTH, fontMap[index]);
// Open the font file
FILE* fp = 0;
if( (fp = fopen(fontPathname, "r")) ==0 ) {
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "BitmapFont::loadTypeFace: unable to open font file: " << fontPathname << std::endl;
}
return 0;
}
// used to store the num of input items successfully matched and assigned
// by fscanf function
int nItemsMatched;
// Calculate the size of the font
unsigned int width1;
nItemsMatched = fscanf(fp, "%u\n", &width1);
unsigned int height1;
nItemsMatched = fscanf(fp, "%u\n", &height1);
unsigned int numBytesWide = int(ceil(double(width1) / 8.0));
unsigned int numFileBytes = numBytesWide * height1;
unsigned int numFontBytes = numBytesWide * getBitmapHeight();
GLubyte* bitmap = new GLubyte[numFontBytes];
unsigned int i; // index
// Pad rest of the height
unsigned int diff = numFontBytes - numFileBytes;
for (i = 0; i < diff; i++) {
bitmap[i] = reverse ? 255 : 0;
}
// Read in the bitmap bytes
for (; i < numFontBytes; i++) {
int value;
nItemsMatched = fscanf(fp, "0x%x\n", &value);
bitmap[i] = reverse ? GLubyte(~value) : GLubyte(value);
}
fclose(fp);
// Reverse the bitmap
reverseBitmapOrder(bitmap, numFontBytes, numBytesWide);
return bitmap;
}
//------------------------------------------------------------------------------
// updateData() -- Update the log file
//------------------------------------------------------------------------------
bool Logger::openFile()
{
// local flags (default is success)
bool tOpened = true;
bool tFailed = false;
// Need a file name
if (filename->len() == 0) {
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "Logger::openFile(): Unable to open logger file: no file name" << std::endl;
}
tOpened = false;
tFailed = true;
}
else {
//---
// Allocate space for the full file name
//---
size_t len = pathname->len(); // start with the length of the path name
len += 1; // add a character for the slash
len += filename->len(); // add the length of the file name
len += 4; // add characters for possible version number, "_V99"
len += 1; // Add one for the null(0) at the end of the string
const size_t NAME_LENGTH = len;
char* fullname = new char[NAME_LENGTH];
fullname[0] = '\0';
//---
// Create the (initial) full file name
//---
if (pathname->len() > 0) {
lcStrcat(fullname,NAME_LENGTH,*pathname);
lcStrcat(fullname,NAME_LENGTH,"/");
}
lcStrcat(fullname,NAME_LENGTH,*filename);
//---
// Make sure that it doesn't already exist (we don't want to over write good data).
//---
bool validName = !doesFileExist(fullname);
if ( !validName ) {
// If the file already exists, try appending a version number "v99" ..
char* origname = new char[NAME_LENGTH];
lcStrcpy(origname, NAME_LENGTH, fullname);
validName = false;
for (unsigned int i = 1; i <= 99 && !validName; i++) {
sprintf(fullname, "%s_v%02d", origname, i);
validName = !doesFileExist(fullname);
}
if ( !validName ) {
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "Logger::openFile(): All version of the logger file already exists: " << origname << std::endl;
}
tOpened = false;
tFailed = true;
}
delete[] origname;
}
if ( validName ) {
//---
// Make sure we have an output stream
//---
if (lout == 0) lout = new std::ofstream();
//---
// Open the file
//---
if (isMessageEnabled(MSG_INFO)) {
std::cout << "Logger::openFile() Opening log file = " << fullname << std::endl;
}
lout->open(fullname);
if (lout->fail()) {
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "Logger::openFile(): Failed to open log file: " << fullname << std::endl;
}
tOpened = false;
tFailed = true;
}
else if (topLine != 0) {
*lout << *topLine << std::endl;
}
}
delete[] fullname;
}
opened = tOpened;
failed = tFailed;
return opened;
}
//------------------------------------------------------------------------------
// Read a record
//------------------------------------------------------------------------------
const DataRecordHandle* FileReader::readRecordImp()
{
DataRecordHandle* handle = nullptr;
// First pass? Does the file need to be opened?
if (firstPassFlg) {
if ( !isOpen() && !isFailed() ) {
openFile();
}
firstPassFlg = false;
}
// When the file is open and ready ...
if ( isOpen() && !isFailed() && !sin->eof() ) {
// Number of bytes in the next serialized DataRecord
unsigned int n = 0;
// ---
// Read the size of the next serialized DataRecord
// ---
char nbuff[8];
sin->read(nbuff, 4);
// Check for error or eof
if ( sin->eof() || sin->fail() ) {
fileFailed = sin->fail();
if (fileFailed && isMessageEnabled(MSG_ERROR | MSG_WARNING)) {
std::cerr << "FileReader::readRecord() -- error reading data record size" << std::endl;
}
}
// Ok then get the size of the message from the buffer
else {
nbuff[4] = '\0';
n = std::atoi(nbuff);
}
// ---
// Read the serialized DataRecord from the file, parse it as a DataRecord
// and put it into a Handle.
// ---
if (n > 0) {
// Read message into ibuf
sin->read(ibuf, n);
// Check for error or eof
if ( sin->eof() || sin->fail() ) {
if (isMessageEnabled(MSG_ERROR | MSG_WARNING)) {
std::cerr << "FileReader::readRecord() -- error reading data record" << std::endl;
}
fileFailed = true;
}
// Ok, create the DataRecord with handle
else {
// Parse the DataRecord
std::string wireFormat(ibuf, n);
Pb::DataRecord* dataRecord = new Pb::DataRecord();
bool ok = dataRecord->ParseFromString(wireFormat);
// Create a handle for the DataRecord (it now has ownership)
if (ok) {
handle = new DataRecordHandle(dataRecord);
}
// parsing error
else if (isMessageEnabled(MSG_ERROR | MSG_WARNING)) {
std::cerr << "FileReader::readRecord() -- ParseFromString() error" << std::endl;
delete dataRecord;
dataRecord = nullptr;
}
}
}
}
return handle;
}
//------------------------------------------------------------------------------
// rfTransmit() -- Transmit a RF emission packet at all active players.
//------------------------------------------------------------------------------
void Antenna::rfTransmit(Emission* const xmit)
{
// Need something to transmit and someone to send to
Tdb* tdb = getCurrentTDB();
Player* ownship = getOwnship();
if (xmit == 0 || tdb == 0 || ownship == 0) {
// Clean up and leave
if (tdb != 0) tdb->unref();
return;
}
// ---
// Compute gimbal boresight data for our targets
// ---
unsigned int ntgts = tdb->computeBoresightData();
if (ntgts > MAX_PLAYERS) ntgts = MAX_PLAYERS;
// ---
// If we have targets
// ---
const osg::Vec3d* losG = tdb->getGimbalLosVectors();
if (ntgts > 0 && losG != 0) {
// ---
// Lookup gain from antenna gain pattern, compute antenna
// effective gain and effective radiated power.
// ---
bool haveGainTgt = false;
double gainTgt[MAX_PLAYERS];
if (gainPattern != 0) {
Basic::Func1* gainFunc1 = dynamic_cast<Basic::Func1*>(gainPattern);
Basic::Func2* gainFunc2 = dynamic_cast<Basic::Func2*>(gainPattern);
if (gainFunc2 != 0) {
// ---
// Antenna pattern: 2D table (az & el off antenna boresight)
// ---
// Compute azimuth off boresight (radians)
const double* aazr = tdb->getBoresightAzimuthErrors();
// Compute elevation off boresight (radians)
const double* aelr = tdb->getBoresightElevationErrors();
// Lookup gain in 2D table and convert from dB
double gainTgt0[MAX_PLAYERS];
if (gainPatternDeg) {
for (unsigned int i1 = 0; i1 < ntgts; i1++) {
gainTgt0[i1] = gainFunc2->f( (aazr[i1] * Basic::Angle::R2DCC), (aelr[i1] * Basic::Angle::R2DCC) )/10.0;
}
}
else {
for (unsigned int i1 = 0; i1 < ntgts; i1++) {
gainTgt0[i1] = gainFunc2->f( aazr[i1], aelr[i1] )/10.0f;
}
}
pow10Array(gainTgt0, gainTgt, ntgts);
haveGainTgt = true;
}
else if (gainFunc1 != 0) {
// ---
// Antenna Pattern: 1D table (off antenna boresight only
// ---
// Compute angles off antenna boresight (radians)
const double* aar = tdb->getBoresightErrorAngles();
// Lookup gain in 1D table and convert from dB
double gainTgt0[MAX_PLAYERS];
if (gainPatternDeg) {
for (unsigned int i2 = 0; i2 < ntgts; i2++) {
gainTgt0[i2] = gainFunc1->f( aar[i2]*Basic::Angle::R2DCC )/10.0;
}
}
else {
for (unsigned int i2 = 0; i2 < ntgts; i2++) {
gainTgt0[i2] = gainFunc1->f( aar[i2] )/10.0f;
}
}
pow10Array(gainTgt0, gainTgt, ntgts);
haveGainTgt = true;
}
}
if (!haveGainTgt) {
// ---
// No antenna pattern table
// ---
for (unsigned int i = 0; i < ntgts; i++) {
gainTgt[i] = 1.0f;
}
}
// Compute antenna effective gain
double aeGain[MAX_PLAYERS];
multArrayConst(gainTgt, getGain(), aeGain, ntgts);
// Compute Effective Radiated Power (watts) (Equation 2-1)
double erp[MAX_PLAYERS];
multArrayConst(aeGain, xmit->getPower(), erp, ntgts);
// Fetch the required data arrays from the TargetDataBlock
const double* ranges = tdb->getTargetRanges();
const double* rngRates = tdb->getTargetRangeRates();
const osg::Vec3d* losO2T = tdb->getLosVectors();
const osg::Vec3d* losT2O = tdb->getTargetLosVectors();
Player** targets = tdb->getTargets();
// ---
// Send emission packets to the targets
// ---
for (unsigned int i = 0; i < ntgts; i++) {
// Only of power exceeds an optional threshold
if (erp[i] > threshold) {
// Get a free emission packet
Emission* em(0);
if (recycle) {
lcLock(freeEmLock);
em = freeEmStack.pop();
lcUnlock(freeEmLock);
}
bool cloned = false;
if (em == 0) {
// Otherwise, clone a new one
em = xmit->clone();
cloned = true;
}
// Send the emission to the other player
if (em != 0) {
// a) Copy the template emission
if (!cloned) *em = *xmit;
// b) Set target unique data
em->setGimbal(this);
em->setOwnship(ownship);
em->setRange( LCreal(ranges[i]) );
em->setLosVec(losO2T[i]);
em->setTgtLosVec(losT2O[i]);
em->setRangeRate( LCreal(rngRates[i]) );
em->setTarget(targets[i]);
em->setGimbalAzimuth( LCreal(getAzimuth()) );
em->setGimbalElevation( LCreal(getElevation()) );
em->setPower( LCreal(erp[i]) );
em->setGain( LCreal(aeGain[i]) );
em->setPolarization(getPolarization());
em->setLocalPlayersOnly( isLocalPlayersOfInterestOnly() );
// c) Send the emission to the target
targets[i]->event(RF_EMISSION, em);
// d) Recycle the emission
bool recycled = false;
if (recycle) {
lcLock(inUseEmLock);
if (inUseEmQueue.isNotFull()) {
// Store for future reference
inUseEmQueue.put(em);
recycled = true;
}
lcUnlock(inUseEmLock);
}
// or just forget it
else {
em->unref();
}
}
else {
// When we couldn't get a free emission packet
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "Antenna: OUT OF EMISSIONS!" << std::endl;
}
}
}
}
}
// Unref() the TDB
tdb->unref();
}