void CExpEq::clear()
{
DBOUT("Try Clear");
cc::clearEquation(value1);
cc::clearEquation(value2);
DBOUT("Clear OK");
}
//-//-// TEST ME !!!
CConstEq* CEqParserV2::_parseNumber(std::string &equation, int &it)
{
CConstEq* rVal = new CConstEq(0);
std::string num;
int charCount = 0;
while ((unsigned)it < equation.length())
{
char chr = equation.at(it);
if (cc::isDecimal(chr))
{
num.push_back(chr);
charCount++;
it++;
}
else
{
DBOUT("NumberString = " << num);
// Happens number ends (i.e. no dec char)
rVal->setValue(std::stod(num));
return rVal;
}
}
DBOUT("NumberString = "<<num);
// Happens when eq.length exceeded
rVal->setValue(charCount>0 ? std::stod(num) : 0);
return rVal;
}
void CAbstractFcnEq::clear()
{
DBOUT("Try Clear");
for (unsigned int i = 0; i < values.size(); i++)
{
cc::clearEquation(values.at(i));
}
values.resize(0);
DBOUT("Clear OK");
}
bool CacheDatabase::updateCacheEntry(const QString &cloud_path, const QString &segment_path, const QString &mapStr)
{
// Query
QSqlQuery query;
// Exec update
bool success = query.exec(QString("UPDATE %1 SET %2='%3',%4='%5' WHERE %6='%7'").arg(CacheTableName, CacheColumnSegmentBase, segment_path, CacheColumnMap, mapStr, CacheColumnCloudPath, cloud_path));
DBOUT("[CacheDatabase] Update sql: " << QString("UPDATE %1 SET %2='%3',%4='%5' WHERE %6='%7'").arg(CacheTableName, CacheColumnSegmentBase, segment_path, CacheColumnMap, mapStr, CacheColumnCloudPath, cloud_path).toStdString());
DBOUT("[CacheDatabase] Update..." << (success?"SUCCESS":"FAIL"));
return success;
}
//-//-// TEST ME !!!
eOpType CEqParserV2::_parseOperator(std::string &equation, int &it)
{
char chr = (unsigned)it < equation.length() ? equation.at(it) : '\0';
it++;
DBOUT("OperatorString = " << chr);
switch (chr)
{
case '+':
return eOpType::ADD_EQ;
break;
case '-':
return eOpType::SUB_EQ;
break;
case '/':
return eOpType::DIV_EQ;
break;
case '*':
return eOpType::MPL_EQ;
break;
case '^':
return eOpType::EXP_EQ;
break;
default:
return eOpType::NULL_EQ;
}
}
static rt_err_t update_mac_address()
{
rt_uint32_t lo,hi;
hi = sep_emac_read(MAC_ADDR1);
lo = sep_emac_read(MAC_ADDR0);
DBOUT("Before MAC: hi=%x lo=%x\n",hi,lo);
sep_emac_write(MAC_ADDR0,(dm9161_device.dev_addr[2] << 24) | (dm9161_device.dev_addr[3] << 16) | (dm9161_device.dev_addr[4] << 8) | (dm9161_device.dev_addr[5]));
sep_emac_write(MAC_ADDR1,dm9161_device.dev_addr[1]|(dm9161_device.dev_addr[0]<<8));
hi = sep_emac_read(MAC_ADDR1);
lo = sep_emac_read(MAC_ADDR0);
DBOUT("After MAC: hi=%x lo=%x\n",hi,lo);
return RT_EOK;
}
void CacheDatabase::saveCacheEntry(const QString &cloud_path, const QString &segment_path, const QString &mapStr)
{
if(insertCacheEntry(cloud_path, segment_path, mapStr) == -1)
{
DBOUT("[CacheDatabase] Insert fail -> update");
updateCacheEntry(cloud_path, segment_path, mapStr);
}
}
CFunctionEq* CEqParserV2::_parseFunction(std::string fcnName, std::string &equation, int &it)
{
//-//-//-//-//-//-//-//-//-//-//-//
// Example: 2+func((2+1),3)+42
// ^
//-//-//-//-//-//-//-//-//-//-//-//
//std::string fcnName = _parseIdentifier(equation, it);
DBOUT("Function name is " << fcnName);
CFunctionEq * fcnEq = new CFunctionEq();
// If function parsing failed or function
// not found return
if (!fcnEq->init(mFcns, fcnName))
{
EROUT("Init Function failed, name not found: "<<fcnName);
return fcnEq;
}
// Parse params -> CChain(2,1),CConst(3)
// (need subequations...)
int paramCount=fcnEq->getParamCount();
DBOUT("Init successful, paramCount is "<<paramCount);
std::vector<CAbstractEq*> params; // XXX unneeded??
// skip first parOpen
if (paramCount>0) it++;
// TODO: Lambda -> paramCount=-1
for (int i = 0; i < paramCount; i++)
{
std::string paramSubstr = _getFcnParamSubstr(equation, it);
DBOUT("Param "<<i<<", substring is: " << paramSubstr);
int subIt = 0;
CAbstractEq* param = _parseEquation(paramSubstr, subIt);
params.push_back(param);
fcnEq->addParamValue(param);
}
return fcnEq;
}
int CacheDatabase::insertCacheEntry(const QString &cloud_path, const QString &segment_path, const QString &mapStr)
{
// Return id
int retId = -1;
// If database's open
if(mDb.open())
{
// The query
QSqlQuery query;
// Exec
bool ret = query.exec(QString("INSERT INTO %1 VALUES ('%2','%3','%4')").arg(CacheTableName,cloud_path, segment_path, mapStr));
DBOUT("[CacheDatabase] Insert sql: " << QString("INSERT INTO %1 VALUES ('%2','%3','%4')").arg(CacheTableName,cloud_path, segment_path, mapStr).toStdString());
// If success
if(ret)
{
// Get the id
retId = query.lastInsertId().toInt();
}
DBOUT("[CacheDatabase] Insert cache data..." << (ret?"DONE":"FAIL:") << ": RetId=" << retId);
}
// Return the id
return retId;
}
bool CacheDatabase::openDB()
{
// Find db driver
mDb = QSqlDatabase::addDatabase("QSQLITE");
// Set path
mDbPath = "tmp/SmogCache.db";
// Setup db
mDb.setDatabaseName(mDbPath);
// Open databse
bool success = mDb.open();
// Log
DBOUT("[CacheDatabase] Open database..." << (success?"DONE":"FAIL"));
// Return success
return success;
}
bool CacheDatabase::prepareDB()
{
// Return value
bool ret = false;
// If the database's opened
if(mDb.isOpen())
{
// Sql query object
QSqlQuery query;
ret = query.exec(QString("CREATE TABLE %1 (%2 TEXT PRIMARY KEY,%3 TEXT,%4 TEXT)").arg(CacheTableName, CacheColumnCloudPath, CacheColumnSegmentBase, CacheColumnMap));
DBOUT("[CacheDatabase] Create table..." << (ret?"DONE":"FAIL"));
}
// Return the result
return ret;
}
std::string CEqParserV2::_parseIdentifier(std::string &equation, int &it)
{
std::string rVal = "";
while ((unsigned)it < equation.length() && cc::isAlpha(equation.at(it)))
{
char chr = equation.at(it);
rVal.push_back(chr);
it++;
}
DBOUT("Parsed Indentifier: " << rVal);
return rVal;
}
double CAbstractFcnEq::getValue()
{
// failsave
if (fcnReference == NULL) return 0;
DBOUT("values.size() = " << values.size());
std::vector<double> tVals;
for (unsigned int i = 0; i < values.size(); i++)
{
tVals.push_back(values.at(i)->getValue());
}
return fcnReference->getValue(tVals);
}
unsigned int Texture2D::Create(std::string const&path, tdelegate& deleg) {
int iComponents;
int width, height;
unsigned char* tex_2d = SOIL_load_image
(
path.c_str(),
&width, &height, &iComponents, SOIL_LOAD_RGBA
);
if (tex_2d == 0) {
const char* description = SOIL_last_result();
DBOUT(description << "\n");
throw BadFormatException(path);
}
unsigned v = deleg(tex_2d, 4, 0, width, height);
SOIL_free_image_data(tex_2d);
return v;
}
//TODO: This is ugly and hacky and needs to get refactored
void OverlayManager::asyncUpdate()
{
boost::lock_guard<boost::mutex> guard(mtx_);
vr::TrackedDeviceIndex_t controller1 = -1;
vr::TrackedDeviceIndex_t controller2 = -1;
vr::VRControllerState_t hmdState;
vr::VRControllerState_t controller1State;
vr::VRControllerState_t controller2State;
vr::TrackedDevicePose_t hmdPose;
vr::TrackedDevicePose_t controller1Pose;
vr::TrackedDevicePose_t controller2Pose;
vr::HmdMatrix34_t overlayTransform;
vr::HmdVector2_t overlayCenter;
vr::ETrackingUniverseOrigin origin;
unsigned int width, height;
//Find the controllers
vr::IVRSystem* vrSys = vr::VRSystem();
vr::IVRCompositor* vrComp = vr::VRCompositor();
vr::IVROverlay* vrOvrly = vr::VROverlay();
for (int i = 0; i < vr::k_unMaxTrackedDeviceCount; i++)
{
switch (vrSys->GetTrackedDeviceClass(i))
{
case vr::TrackedDeviceClass_Controller:
if (controller1 == -1)
{
controller1 = i;
}
if (controller1 >= 0 && i !=controller1)
{
controller2 = i;
}
if (controller2 >= 0)
{
break;
}
}
}
int count = 0;
for (std::vector<std::shared_ptr<Overlay>>::iterator it = m_overlayVec.begin(); it != m_overlayVec.end(); ++it)
{
if (vrSys && controller1 >= 0 && (*it)->isVisible())
{
//Set padding of the overlay based on scale
float padding = 0.5f * ((float)(*it)->getScale() / 100.0f);
float z_padding = 0.1f;
//Get the controller pose information relative to tracking space
vrSys->GetControllerStateWithPose(vrComp->GetTrackingSpace(), controller1, &controller1State, sizeof(controller1State), &controller1Pose);
vrSys->GetControllerStateWithPose(vrComp->GetTrackingSpace(), controller2, &controller2State, sizeof(controller2State), &controller2Pose);
vrSys->GetControllerStateWithPose(vrComp->GetTrackingSpace(), vr::k_unTrackedDeviceIndex_Hmd, &hmdState, sizeof(hmdState), &hmdPose);
//Center of the overlay adjusted for scale
overlayCenter.v[0] = 0.5f;// * ((float)(*it)->getScale() / 100.0f);
overlayCenter.v[1] = 0.5f;// * ((float)(*it)->getScale() / 100.0f);
//Get the overlay transform relative to tracking space
vr::EVROverlayError err = vr::VROverlayError_None;
if (err = vrOvrly->GetTransformForOverlayCoordinates((*it)->getOverlayHandle(), vrComp->GetTrackingSpace(), overlayCenter, &overlayTransform))
{
DBOUT("Error with overlay!!" << err << std::endl);
}
//Converts Controller world tracking transform matrix to a transform matrix relative to the overlay
Eigen::Transform<float, 3, Eigen::Affine> controller1Transform;
Eigen::Transform<float, 3, Eigen::Affine> controller2Transform;
Eigen::Transform<float, 3, Eigen::Affine> overlayTrans;
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 4; ++j)
{
controller1Transform(i, j) = controller1Pose.mDeviceToAbsoluteTracking.m[i][j];
controller2Transform(i, j) = controller2Pose.mDeviceToAbsoluteTracking.m[i][j];
overlayTrans(i, j) = overlayTransform.m[i][j];
}
}
Eigen::Matrix<float, 4, 4> overlayInverse = overlayTrans.matrix().inverse();
Eigen::Matrix<float, 4, 4> controller1OverlayTransform = overlayInverse * controller1Transform.matrix();
Eigen::Matrix<float, 4, 4> controller2OverlayTransform = overlayInverse * controller2Transform.matrix();
//Boolean values for if the controller is within the bounds of the overlay based on the padding
//z-padding is used for the depth across the face of the overlay
bool controller1InOverlay = (controller1OverlayTransform(0, 3) < padding && controller1OverlayTransform(0, 3) > -padding) &&
(controller1OverlayTransform(1, 3) < padding && controller1OverlayTransform(1, 3) > -padding) &&
(controller1OverlayTransform(2, 3) < z_padding && controller1OverlayTransform(2, 3) > -z_padding);
bool controller2InOverlay = (controller2OverlayTransform(0, 3) < padding && controller2OverlayTransform(0, 3) > -padding) &&
(controller2OverlayTransform(1, 3) < padding && controller2OverlayTransform(1, 3) > -padding) &&
(controller2OverlayTransform(2, 3) < z_padding && controller2OverlayTransform(2, 3) > -z_padding);;
//If the controller is within the bounds the center of the overlay
if (controller1InOverlay || controller2InOverlay)
{
//Buzz controller -- Not working fix
vr::VRSystem()->TriggerHapticPulse(controller1, 2, 2000);
if (controller1InOverlay && (*it)->getTracking() != 2)
{
//If controller1 is not currently tracking and controller2 isn't tracking overlay
if(controller1State.rAxis[1].x > 0.75f && (m_controller1Tracking == NULL || m_controller1Tracking == (*it).get()) && m_controller2Tracking != (*it).get())
{
TrackingUpdate(it, controller1State, controller1Pose, controller1InOverlay, vrSys, vrComp);
m_controller1Tracking = (*it).get();
emit textureUpdated(count);
}
//If trigger is released and was tracking, reset pointer to null;
if (controller1State.rAxis[1].x < 0.75f && m_controller1Tracking != NULL)
{
m_controller1Tracking = NULL;
}
//If touchpad is pressed in overlay w/ debounce check
if (((controller1State.ulButtonPressed & vr::ButtonMaskFromId(vr::k_EButton_SteamVR_Touchpad)) == vr::ButtonMaskFromId(vr::k_EButton_SteamVR_Touchpad)) &&
!m_controller1TouchPressed)
{
m_controller1TouchPressed = true;
//bottom left - decrease opacity
if (controller1State.rAxis[0].x < 0 && controller1State.rAxis[0].y < 0)
(*it)->setTransparancy((*it)->getTransparancy() - 5);
//bottom right - increase opacity
if (controller1State.rAxis[0].x > 0 && controller1State.rAxis[0].y < 0)
(*it)->setTransparancy((*it)->getTransparancy() + 5);
//top left - decrease scale
if (controller1State.rAxis[0].x < 0 && controller1State.rAxis[0].y > 0)
(*it)->setScale((*it)->getScale() - 5);
//top right - increase scale
if (controller1State.rAxis[0].x > 0 && controller1State.rAxis[0].y > 0)
(*it)->setScale((*it)->getScale() + 5);
emit textureUpdated(count);
}
else if (((controller1State.ulButtonPressed & vr::ButtonMaskFromId(vr::k_EButton_SteamVR_Touchpad)) != vr::ButtonMaskFromId(vr::k_EButton_SteamVR_Touchpad)) &&
m_controller1TouchPressed)
{
m_controller1TouchPressed = false;
}
//If SideButton is pressed
else if (((controller1State.ulButtonPressed & vr::ButtonMaskFromId(vr::k_EButton_Grip)) == vr::ButtonMaskFromId(vr::k_EButton_Grip)) &&
!m_controller1GripPressed)
{
m_controller1GripPressed = true;
(*it)->setTracking(((*it)->getTracking() + 1) % 4);
emit textureUpdated(count);
}
else if (((controller1State.ulButtonPressed & vr::ButtonMaskFromId(vr::k_EButton_Grip)) != vr::ButtonMaskFromId(vr::k_EButton_Grip)) &&
m_controller1GripPressed)
{
m_controller1GripPressed = false;
}
}
if (controller2InOverlay && (*it)->getTracking() != 3)
{
//If controller2 is not currently tracking and controller1 isn't tracking overlay
if (controller2State.rAxis[1].x > 0.75f && (m_controller2Tracking == NULL || m_controller2Tracking == (*it).get()) && m_controller1Tracking != (*it).get())
{
TrackingUpdate(it, controller2State, controller2Pose, controller2InOverlay, vrSys, vrComp);
m_controller2Tracking = (*it).get();
emit textureUpdated(count);
}
if (controller2State.rAxis[1].x < 0.75f && m_controller2Tracking != NULL)
{
m_controller2Tracking = NULL;
}
//If touchpad is pressed in overlay w/ debounce check
if (((controller2State.ulButtonPressed & vr::ButtonMaskFromId(vr::k_EButton_SteamVR_Touchpad)) == vr::ButtonMaskFromId(vr::k_EButton_SteamVR_Touchpad)) &&
!m_controller2TouchPressed)
{
m_controller2TouchPressed = true;
//bottom left - decrease opacity
if (controller2State.rAxis[0].x < 0 && controller2State.rAxis[0].y < 0)
(*it)->setTransparancy((*it)->getTransparancy() - 5);
//bottom right - increase opacity
if (controller2State.rAxis[0].x > 0 && controller2State.rAxis[0].y < 0)
(*it)->setTransparancy((*it)->getTransparancy() + 5);
//top left - decrease scale
if (controller2State.rAxis[0].x < 0 && controller2State.rAxis[0].y > 0)
(*it)->setScale((*it)->getScale() - 5);
//top right - increase scale
if (controller2State.rAxis[0].x > 0 && controller2State.rAxis[0].y > 0)
(*it)->setScale((*it)->getScale() + 5);
emit textureUpdated(count);
}
else if (((controller2State.ulButtonPressed & vr::ButtonMaskFromId(vr::k_EButton_SteamVR_Touchpad)) != vr::ButtonMaskFromId(vr::k_EButton_SteamVR_Touchpad)) &&
m_controller2TouchPressed)
{
m_controller2TouchPressed = false;
}
//If SideButton is pressed
else if (((controller2State.ulButtonPressed & vr::ButtonMaskFromId(vr::k_EButton_Grip)) == vr::ButtonMaskFromId(vr::k_EButton_Grip)) &&
!m_controller2GripPressed)
{
m_controller2GripPressed = true;
(*it)->setTracking(((*it)->getTracking() + 1) % 4);
}
else if (((controller2State.ulButtonPressed & vr::ButtonMaskFromId(vr::k_EButton_Grip)) != vr::ButtonMaskFromId(vr::k_EButton_Grip)) &&
m_controller2GripPressed)
{
m_controller2GripPressed = false;
}
}
} //end controller in overlay if check
} //end VR check if
count++;
} //end iterator loop for overlays
m_timer.expires_from_now(boost::posix_time::milliseconds(5));
m_timer.async_wait(boost::bind(&OverlayManager::asyncUpdate, this));
}
void CloudStore::filterVisibleCloudsTo(const math::Polygonf& polygon, const QString &filepath)
{
try
{
/* Old solution: copy from all file
// Las output
std::ofstream ofs(filepath.toStdString().c_str(), ios::out | ios::binary);
std::shared_ptr<liblas::Writer> writerprt;
for(CloudEntry::Ptr& cloud : mClouds)
{
// Only if visible
if(!cloud->isVisible())
continue;
// Only if .las
QFileInfo fileinfo(cloud->getFilePath());
if(fileinfo.suffix() != "las")
continue;
// Las input from cloud->getFilePath()
std::ifstream ifs(cloud->getFilePath().toStdString().c_str(), ios::in | ios::binary);
liblas::Reader reader(ifs);
// Create writer
if(!writerprt)
{
writerprt.reset(new liblas::Writer(ofs, reader.GetHeader()));
DBOUT("[Filter] Writer header: " << std::endl << writerprt->GetHeader());
}
// Print header
DBOUT("[Filter] Reader Header:" << std::endl << reader.GetHeader());
// Copy
while(reader.ReadNextPoint())
{
liblas::Point point = reader.GetPoint();
math::Vector2f filterPoint(point.GetX(), point.GetY());
if(math::isPointInsidePolygon(polygon,filterPoint))
{
// Correcting offset to the writer offset
point.SetX(point.GetX() + (writerprt->GetHeader().GetOffsetX() - reader.GetHeader().GetOffsetX()));
point.SetY(point.GetY() + (writerprt->GetHeader().GetOffsetY() - reader.GetHeader().GetOffsetY()));
point.SetZ(point.GetZ() + (writerprt->GetHeader().GetOffsetZ() - reader.GetHeader().GetOffsetZ()));
// Write point
writerprt->WritePoint(point);
}
}
}
*/
/// New solution: as lasmerge
/// Step 0: collect cloud references
std::vector<CloudEntry::Ptr> cloudsToCopy;
for(CloudEntry::Ptr& cloud : mClouds)
{
QFileInfo fileinfo(cloud->getFilePath());
if(cloud->isVisible() && fileinfo.suffix() == "las")
cloudsToCopy.push_back(cloud);
}
// If no visible cloud -> return
if(cloudsToCopy.empty())
return;
/// Step 1: create merged header
// The merged header
liblas::Header mergedHeader;
// Read header from all the file
for(size_t i = 0; i < cloudsToCopy.size(); ++i)
{
// Cloud reference
CloudEntry::Ptr cloud = cloudsToCopy[i];
// Input stream for the reader
std::ifstream ifs(cloud->getFilePath().toStdString().c_str(), ios::in | ios::binary);
// Create reader for that file
liblas::Reader reader(ifs);
// Get header
const liblas::Header& header = reader.GetHeader();
// If it's the first, set as merged
if(i == 0)
{
mergedHeader = header;
}
// Else update the merged header
else
{
// Set id
mergedHeader.SetSystemId("MERGED");
// Set points records count
mergedHeader.SetPointRecordsCount(mergedHeader.GetPointRecordsCount() + header.GetPointRecordsCount());
// Set point reccords by return
for(size_t i = 0; i < std::min(mergedHeader.GetPointRecordsByReturnCount().size(),header.GetPointRecordsByReturnCount().size()); ++i)
mergedHeader.SetPointRecordsByReturnCount(i, mergedHeader.GetPointRecordsByReturnCount()[i] + header.GetPointRecordsByReturnCount()[i]);
// Set bounds
mergedHeader.SetMin(std::min(mergedHeader.GetMinX(), header.GetMinX()), std::min(mergedHeader.GetMinY(), header.GetMinY()), std::min(mergedHeader.GetMinZ(), header.GetMinZ()));
mergedHeader.SetMax(std::max(mergedHeader.GetMaxX(), header.GetMaxX()), std::max(mergedHeader.GetMaxY(), header.GetMaxY()), std::max(mergedHeader.GetMaxZ(), header.GetMaxZ()));
}
}
/// Step 2: Update scale
// Limits
int smallest_int = (1<<31)+10;
int largest_int = smallest_int-1-20;
// Pack infos into vectors
math::Vector3d scale(mergedHeader.GetScaleX(), mergedHeader.GetScaleY(), mergedHeader.GetScaleZ());
math::Vector3d offset(mergedHeader.GetOffsetX(), mergedHeader.GetOffsetY(), mergedHeader.GetOffsetZ());
math::Vector3d min(mergedHeader.GetMinX(), mergedHeader.GetMinY(), mergedHeader.GetMinZ());
math::Vector3d max(mergedHeader.GetMaxX(), mergedHeader.GetMaxY(), mergedHeader.GetMaxZ());
// Correcting scales
for(size_t i = 0; i < 3; ++i)
{
if((max[i] - offset[i]) / scale[i] > largest_int || (min[i] - offset[i]) / scale[i] < smallest_int)
{
scale[i] = 0.0000001;
while((max[i] - offset[i]) / scale[i] > largest_int || (min[i] - offset[i]) / scale[i] < smallest_int)
scale[i] *= 10;
}
}
// Set new scale
mergedHeader.SetScale(scale.x, scale.y, scale.z);
/// Step 3: write point to the output
DBOUT("[Filter] Merged header: " << std::endl << mergedHeader);
// Output stream
std::ofstream ofs(filepath.toStdString().c_str(), ios::out | ios::binary);
// Create writer
liblas::Writer writer(ofs, mergedHeader);
// For all input
for(size_t i = 0; i < cloudsToCopy.size(); ++i)
{
// Cloud pointer
CloudEntry::Ptr cloud = cloudsToCopy[i];
// Input stream
std::ifstream ifs(cloud->getFilePath().toStdString().c_str(), ios::in | ios::binary);
// Create reader
liblas::Reader reader(ifs);
// Get header
const liblas::Header& header = reader.GetHeader();
// Log
DBOUT("[Filter] Header: " << std::endl << header);
// Same offset and scale
bool same = true;
// Chack if the same
if(mergedHeader.GetOffsetX() != header.GetOffsetX()) same = false;
if(mergedHeader.GetOffsetY() != header.GetOffsetY()) same = false;
if(mergedHeader.GetOffsetZ() != header.GetOffsetZ()) same = false;
if(mergedHeader.GetScaleX() != header.GetScaleX()) same = false;
if(mergedHeader.GetScaleY() != header.GetScaleY()) same = false;
if(mergedHeader.GetScaleZ() != header.GetScaleZ()) same = false;
// All point from this cloud
while(reader.ReadNextPoint())
{
// Get point
liblas::Point point = reader.GetPoint();
// If the point isn't inside the polygon
if(!math::isPointInsidePolygon(polygon, math::Vector2f(point.GetX(), point.GetY())))
continue;
// Correction if not the same
if(!same)
{
point.SetX(point.GetX() - (mergedHeader.GetOffsetX() - header.GetOffsetX()));
point.SetY(point.GetY() - (mergedHeader.GetOffsetY() - header.GetOffsetY()));
point.SetZ(point.GetZ() - (mergedHeader.GetOffsetZ() - header.GetOffsetZ()));
/*point.SetX(0.5 + (point.GetX() - mergedHeader.GetOffsetX()) / mergedHeader.GetScaleX());
point.SetY(0.5 + (point.GetY() - mergedHeader.GetOffsetY()) / mergedHeader.GetScaleY());
point.SetZ(0.5 + (point.GetZ() - mergedHeader.GetOffsetZ()) / mergedHeader.GetScaleZ());*/
}
// Write the point
writer.WritePoint(point);
}
}
}
catch (std::exception const& e)
{
std::cerr << "Error: " << e.what() << std::endl;
}
}
CAbstractEq* CEqParserV2::_parseEquation(std::string &equation, int &it)
{
CChainEq* rVal = new CChainEq();
CAbstractEq* number = NULL;
bool hasNum = false;
bool hasOp = false;
bool inverseNum = false;
char chr = equation.at(it);
while ((unsigned)it < equation.length())
{
//// Fast exit if statement ends
//if(cc::isParClose(equation.at(it)))
//{
// DBOUT("Called break");
// it+=2;
// break;
//}
chr = equation.at(it);
DBOUT("it = " << it << ", char = " << chr);
eOpType op = eOpType::NULL_EQ;
if (cc::isDecimal(chr))
{
DBOUT("Found decimal: " << chr);
number = _parseNumber(equation, it);
}
else if (cc::isParOpen(chr))
{
std::string sub = _getEqSubstr(equation, ++it);
DBOUT("Found subEquation "<<sub);
int subIt = 0; // dummy
number = _parseEquation(sub, subIt);
}
else if (cc::isAlpha(chr))
{
DBOUT("Found alpha " << chr);
std::string fcnName = _parseIdentifier(equation, it);
number = _parseFunction(fcnName,equation, it);
}
else if (cc::isOperator(chr))
{
op = _parseOperator(equation, it);
}
else
{
it++;
}
hasNum = (number != NULL);
hasOp = (op != eOpType::NULL_EQ);
DBOUT("hasOp = " << hasOp << " hasNum = " << hasNum);
if (hasNum)
{
bool doAdd = false;
// Has Number and Operator
if (hasOp)
{
doAdd = true;
}
// Default Operator "CONST" if finished
else if ((unsigned)it >= equation.length())
{
op = eOpType::CONST_EQ;
doAdd = true;
}
// Default Operator "MPL" if undefined
else
{
chr = equation.at(it);
DBOUT("Try autoadd MPL (203) char is: " << chr);
// Subequation e.g. 4(2+3)
// ^ ^
doAdd |= cc::isParOpen(chr);
// Function e.g. 4x
// ^
doAdd |= cc::isAlpha(chr);
// Decimal e.g. (2+3)4
// ^
doAdd |= cc::isDecimal(chr);
if (doAdd)
{
op = eOpType::MPL_EQ;
}
}
if (doAdd)
{
if (inverseNum) number = new CInvEq(number);
DBOUT("Adding Operation "<<op);
rVal->addOperation(number, op);
op = eOpType::NULL_EQ;
number = NULL;
//
hasNum = false;
hasOp = false;
inverseNum = false;
}
}
// Inverse negative
else
{
if (op == eOpType::SUB_EQ)
{
DBOUT("Setting inversenum = true");
inverseNum = true;
}
}
}
if (hasNum)
{
if (inverseNum) number = new CInvEq(number);
DBOUT("Adding Operation 101");
rVal->addOperation(number, eOpType::CONST_EQ);
}
DBOUT("Returning equation, it = "<<it);
return rVal;
}
static void update_link_speed(unsigned short phy_addr)
{
unsigned int bmsr, bmcr, lpa, mac_cfg;
unsigned int speed, duplex;
if (!mii_link_ok(phy_addr))
{
EOUT("Link Down\n");
//goto result;
}
read_phy(phy_addr,MII_BMSR,&bmsr);
read_phy(phy_addr,MII_BMCR,&bmcr);
if (bmcr & BMCR_ANENABLE) /* AutoNegotiation is enabled */
{
if (!(bmsr & BMSR_ANEGCOMPLETE)) /* Do nothing - another interrupt generated when negotiation complete */
goto result;
read_phy(phy_addr, MII_LPA, &lpa);
if ((lpa & LPA_100FULL) || (lpa & LPA_100HALF))
speed = SPEED_100;
else
speed = SPEED_10;
if ((lpa & LPA_100FULL) || (lpa & LPA_10FULL))
duplex = DUPLEX_FULL;
else
duplex = DUPLEX_HALF;
}
else
{
speed = (bmcr & BMCR_SPEED100) ? SPEED_100 : SPEED_10;
duplex = (bmcr & BMCR_FULLDPLX) ? DUPLEX_FULL : DUPLEX_HALF;
}
/* Update the MAC */
mac_cfg = sep_emac_read(MAC_CTRL);
if (speed == SPEED_100)
{
mac_cfg |= 0x800; /* set speed 100 M */
//bmcr &=(~0x2000);
//write_phy(lp->phy_address, MII_BMCR, bmcr); //将dm9161的速度设为10M
if (duplex == DUPLEX_FULL) /* 100 Full Duplex */
mac_cfg |= 0x400;
else /* 100 Half Duplex */
mac_cfg &= (~0x400);
}
else
{
mac_cfg &= (~0x800); /* set speed 10 M */
if (duplex == DUPLEX_FULL) /* 10 Full Duplex */
mac_cfg |= 0x400;
else /* 10 Half Duplex */
mac_cfg &= (~0x400);
}
sep_emac_write(MAC_CTRL, mac_cfg);
rt_kprintf("Link now %i M-%s\n", speed, (duplex == DUPLEX_FULL) ? "FullDuplex" : "HalfDuplex");
result:
mac_cfg = sep_emac_read(MAC_CTRL);
DBOUT("After mac_cfg=%d\n",mac_cfg);
return;
}
