std::pair<arma::vec2, arma::vec2> Quad::getIntersectionPoints(Line line) const {
std::pair<arma::vec2, arma::vec2> points;
std::vector<ParametricLine<>> quadLines = {ParametricLine<>(tl, tr, true),
ParametricLine<>(tr, br, true),
ParametricLine<>(bl, br, true),
ParametricLine<>(bl, tl, true)};
int counter = 0;
ParametricLine<> pLine(line.pointFromTangentialDistance(0), line.pointFromTangentialDistance(1));
for (auto& quadLine : quadLines) {
try {
if (counter == 0) {
points.first = pLine.intersect(quadLine);
counter++;
}
else {
points.second = pLine.intersect(quadLine);
counter++;
break;
}
}
catch (std::domain_error&) {
// did not intersect, ignore
}
}
if (counter < 2) {
throw std::domain_error("Quad::intersect - Line does not not intersect quad");
}
return points;
}
Array2D<T>::Array2D(const Array2D& rhs)
{
height = rhs.height;
width = rhs.width;
length = rhs.length;
pLine( new T[height * width]);
for(unsigned long long int i=0; i<length; ++i)
{
(*pLine)[i] = (*rhs.pLine)[i];
}
}
bool PNS_DIFF_PAIR_PLACER::rhShoveOnly( const VECTOR2I& aP )
{
m_currentNode = m_shove->CurrentNode();
bool ok = routeHead( aP );
m_fitOk = false;
if( !ok )
return false;
if( !tryWalkDp( m_currentNode, m_currentTrace, true ) )
return false;
PNS_LINE pLine( m_currentTrace.PLine() );
PNS_LINE nLine( m_currentTrace.NLine() );
PNS_ITEMSET head;
head.Add( &pLine );
head.Add( &nLine );
PNS_SHOVE::SHOVE_STATUS status = m_shove->ShoveMultiLines( head );
m_currentNode = m_shove->CurrentNode();
if( status == PNS_SHOVE::SH_OK )
{
m_currentNode = m_shove->CurrentNode();
if( !m_currentNode->CheckColliding( &m_currentTrace.PLine() ) &&
!m_currentNode->CheckColliding( &m_currentTrace.NLine() ) )
{
m_fitOk = true;
}
}
return m_fitOk;
}
int ColorLUT::generate(ColorModel *model, const Frame8 &frame, const RectA ®ion)
{
Fpoint meanVal;
float angle, pangle, pslope, meanSat;
float yi, istep, s, xsat, sat;
int result;
m_hpixels = (HuePixel *)maxMalloc(sizeof(HuePixel)*CL_HPIXEL_MAX_SIZE, &m_hpixelSize);
if (m_hpixels==NULL)
return -1; // not enough memory
m_hpixelSize /= sizeof(HuePixel);
map(frame, region);
mean(&meanVal);
angle = atan2(meanVal.m_y, meanVal.m_x);
Fpoint uvec(cos(angle), sin(angle));
Line hueLine(tan(angle), 0.0);
pangle = angle + PI/2; // perpendicular angle
pslope = tan(pangle); // perpendicular slope
Line pLine(pslope, meanVal.m_y - pslope*meanVal.m_x); // perpendicular line through mean
// upper hue line
istep = fabs(m_iterateStep/uvec.m_x);
yi = iterate(hueLine, istep);
yi += fabs(m_hueTol*yi); // extend
model->m_hue[0].m_yi = yi;
model->m_hue[0].m_slope = hueLine.m_slope;
// lower hue line
yi = iterate(hueLine, -istep);
yi -= fabs(m_hueTol*yi); // extend
model->m_hue[1].m_yi = yi;
model->m_hue[1].m_slope = hueLine.m_slope;
// inner sat line
s = sign(uvec.m_y);
istep = s*fabs(m_iterateStep/cos(pangle));
yi = iterate(pLine, -istep);
yi -= s*fabs(m_satTol*(yi-pLine.m_yi)); // extend
xsat = yi/(hueLine.m_slope-pslope); // x value where inner sat line crosses hue line
Fpoint minsatVec(xsat, xsat*hueLine.m_slope); // vector going to inner sat line
sat = dot(uvec, minsatVec); // length of line
meanSat = dot(uvec, meanVal);
if (sat < m_minSat) // if it's too short, we need to extend
{
minsatVec.m_x = uvec.m_x*m_minSat;
minsatVec.m_y = uvec.m_y*m_minSat;
yi = minsatVec.m_y - pslope*minsatVec.m_x;
}
model->m_sat[0].m_yi = yi;
model->m_sat[0].m_slope = pslope;
// outer sat line
yi = iterate(pLine, istep);
yi += s*fabs(m_maxSatRatio*m_satTol*(yi-pLine.m_yi)); // extend
model->m_sat[1].m_yi = yi;
model->m_sat[1].m_slope = pslope;
// swap if outer sat line is greater than inner sat line
// Arbitrary convention, but we need it to be consistent to test membership (checkBounds)
if (model->m_sat[1].m_yi>model->m_sat[0].m_yi)
{
Line tmp = model->m_sat[0];
model->m_sat[0] = model->m_sat[1];
model->m_sat[1] = tmp;
}
free(m_hpixels);
// calculate goodness
result = (meanSat-m_minSat)*100/64 + 10; // 64 because it's half of our range
if (result<0)
result = 0;
if (result>100)
result = 100;
return result;
}
bool CLoadBmp::Write(const CString &strType, IW::IStreamOut *pStreamOut, const IW::Image &imageIn, IW::IStatus *pStatus)
{
CString str;
str.Format(IDS_ENCODING_FMT, GetTitle());
pStatus->SetStatusMessage(str);
BITMAPFILEHEADER hdr;
IW::Page page = imageIn.GetFirstPage();
IW::PixelFormat pf = page.GetPixelFormat();
int cx = page.GetWidth();
int cy = page.GetHeight();
int nPaletteEntries = pf.NumberOfPaletteEntries();
int nStorageWidth = IW::CalcStorageWidth(cx, pf);
int nSizeImage = nStorageWidth * cy;
int nSizeHeader = sizeof(BITMAPINFOHEADER) + nPaletteEntries * sizeof(RGBQUAD);
// Fill in the fields of the file header
hdr.bfType = 0x4d42;
hdr.bfSize = nSizeImage + nSizeHeader + sizeof(BITMAPFILEHEADER);
hdr.bfReserved1 = 0;
hdr.bfReserved2 = 0;
hdr.bfOffBits = (DWORD) (sizeof(BITMAPFILEHEADER) + nSizeHeader);
pStreamOut->Write((LPSTR)&hdr, (UINT)(sizeof(BITMAPFILEHEADER)), 0);
// Fill in the header info.
BITMAPINFOHEADER bmh;
bmh.biSize = sizeof(BITMAPINFOHEADER);
bmh.biWidth = cx;
bmh.biHeight = cy;
bmh.biPlanes = 1;
bmh.biBitCount = pf.ToBpp();
bmh.biCompression = BI_RGB;
bmh.biSizeImage = nSizeImage;
bmh.biXPelsPerMeter = imageIn.GetXPelsPerMeter();
bmh.biYPelsPerMeter = imageIn.GetYPelsPerMeter();
bmh.biClrUsed = nPaletteEntries;
bmh.biClrImportant = 0;
// this struct already DWORD aligned!
// Write the DIB header and the bits
pStreamOut->Write((LPSTR)&bmh, sizeof(bmh), 0);
pStreamOut->Write((LPSTR)page.GetPalette(), nPaletteEntries * sizeof(RGBQUAD), 0);
if (pf == IW::PixelFormat::PF32Alpha || pf == IW::PixelFormat::PF32)
{
IW::ConstIImageSurfaceLockPtr pLock = page.GetSurfaceLock();
IW::CAutoFree<COLORREF> pLine(cx + 1);
for (int y = 0; y < cy; ++y)
{
pLock->RenderLine(pLine, cy - (y + 1), 0, cx);
pStreamOut->Write(pLine, nStorageWidth, 0);
}
}
else
{
for (int y = 0; y < cy; ++y)
{
pStreamOut->Write((LPSTR)page.GetBitmapLine(cy - (y + 1)), nStorageWidth, 0);
}
}
return TRUE;
}
int testPlane() {
int numErr = 0;
logMessage(_T("TESTING - class GM_3dPlane ...\n\n"));
// Default constructor, plane must be invalid
GM_3dPlane p;
if (p.isValid()) {
logMessage(_T("\tERROR - Default constructor creates valid plane\n"));
numErr++;
}
else {
logMessage(_T("\tOK - Default constructor creates invalid plane\n"));
}
// Constructor (coeff)
GM_3dPlane p1(getRandomDouble(), getRandomDouble(), getRandomDouble(), getRandomDouble());
GM_3dPlane pNull(0.0, 0.0, 0.0, getRandomDouble());
if (!p1.isValid() || pNull.isValid()) {
logMessage(_T("\tERROR - Coeff. constructor not working\n"));
numErr++;
}
else {
logMessage(_T("\tOK - Coeff. constructor working\n"));
}
// Copy constructor
GM_3dPlane copyPlane(p1);
if (!copyPlane.isValid() || copyPlane != p1) {
logMessage(_T("\tERROR - Copy constructor not working\n"));
numErr++;
}
else {
logMessage(_T("\tOK - Copy constructor working\n"));
}
// Constructor (coeff vector)
double pointsVect[4];
for (int i = 0 ; i < 4 ; i++) {
pointsVect[i] = getRandomDouble();
}
GM_3dPlane p2(pointsVect);
if (!p2.isValid()) {
logMessage(_T("\tERROR - Coeff. vector constructor not working\n"));
numErr++;
}
else {
logMessage(_T("\tOK - Coeff. vector constructor working\n"));
}
// Constructor (points)
GM_3dPoint pt1(getRandomDouble(), getRandomDouble(), getRandomDouble());
GM_3dPoint pt2(getRandomDouble(), getRandomDouble(), getRandomDouble());
GM_3dPoint pt3(getRandomDouble(), getRandomDouble(), getRandomDouble());
GM_3dLine ln(getRandomDouble(), getRandomDouble(), getRandomDouble(),getRandomDouble(), getRandomDouble(), getRandomDouble());
GM_3dPoint ptLn1 = ln.begin();
GM_3dPoint ptLn2 = ln.center();
GM_3dPoint ptLn3 = ln.end();
GM_3dPlane p3(pt1, pt2, pt3);
GM_3dPlane pLine(ptLn2, ptLn2, ptLn3);
double distPt1 = pt1.x()*p3[0] + pt1.y()*p3[1] + pt1.z()*p3[2] + p3[3];
double distPt2 = pt2.x()*p3[0] + pt2.y()*p3[1] + pt2.z()*p3[2] + p3[3];
double distPt3 = pt3.x()*p3[0] + pt3.y()*p3[1] + pt3.z()*p3[2] + p3[3];
if (!p3.isValid() || pLine.isValid() || fabs(distPt1) > GM_NULL_TOLERANCE || fabs(distPt2) > GM_NULL_TOLERANCE || fabs(distPt3) > GM_NULL_TOLERANCE) {
logMessage(_T("\tERROR - Points constructor not working\n"));
numErr++;
}
else {
logMessage(_T("\tOK - Points constructor working\n"));
}
// Point distance
GM_3dPlane p4(getRandomDouble(), getRandomDouble(), getRandomDouble(),getRandomDouble());
GM_3dPoint checkPoint(getRandomDouble(), getRandomDouble(), getRandomDouble());
if (fabs(p4[0]) > GM_NULL_TOLERANCE) {
checkPoint.x(-(checkPoint.y()*p4[1] + checkPoint.z()*p4[2] + p4[3]) / p4[0]);
}
else if (fabs(p4[1]) > GM_NULL_TOLERANCE) {
checkPoint.y(-(checkPoint.x()*p4[0] + checkPoint.z()*p4[2] + p4[3]) / p4[1]);
}
else if (fabs(p4[2]) > GM_NULL_TOLERANCE) {
checkPoint.z(-(checkPoint.x()*p4[0] + checkPoint.y()*p4[1] + p4[3]) / p4[2]);
}
else {
p4.invalidate();
}
double checkSignedDist = getRandomDouble();
double checkDist = fabs(checkSignedDist);
GM_3dVector p4Norm = p4.normalVector();
checkPoint = (GM_3dVector)checkPoint + (p4Norm * checkSignedDist);
GM_3dPoint checkPointOnPlane1;
GM_3dPoint checkPointOnPlane2;
double dist = p4.pointDistance(checkPoint);
double signedDist = p4.pointDistanceSgn(checkPoint);
double signedDistOnPlane = p4.pointDistanceSgn(checkPoint, checkPointOnPlane1);
double distOnPlane = p4.pointDistance(checkPoint, checkPointOnPlane2);
distPt1 = checkPointOnPlane1.x()*p4[0] + checkPointOnPlane1.y()*p4[1] + checkPointOnPlane1.z()*p4[2] + p4[3];
distPt2 = checkPointOnPlane2.x()*p4[0] + checkPointOnPlane2.y()*p4[1] + checkPointOnPlane2.z()*p4[2] + p4[3];
if (!p4.isValid() || !checkPointOnPlane1.isValid() || !checkPointOnPlane2.isValid() ||
fabs(distPt1) > GM_NULL_TOLERANCE || fabs(distPt2) > GM_NULL_TOLERANCE ||
fabs(distOnPlane - checkDist) > GM_NULL_TOLERANCE || fabs(signedDistOnPlane - checkSignedDist) > GM_NULL_TOLERANCE ||
fabs(dist - checkDist) > GM_NULL_TOLERANCE || fabs(signedDist - checkSignedDist) > GM_NULL_TOLERANCE) {
logMessage(_T("\tERROR - Point distance computation not working\n"));
numErr++;
}
else {
logMessage(_T("\tOK - Point distance computation working\n"));
}
// XY Angle
double angle = ((((double)rand()) / ((double)RAND_MAX)) * GM_PI) - (GM_PI / 2.0);
GM_3dVector normVector(angle/* + GM_HALFPI*/);
normVector.z(normVector.y());
normVector.y(0.0);
GM_3dPlane angleP(normVector, GM_3dPoint(getRandomDouble(), getRandomDouble(), getRandomDouble()));
double checkAngle = angleP.xyAngle();
if (checkAngle > GM_PI) {
checkAngle -= 2.0 * GM_PI;
}
if (!angleP.isValid() || fabs(angle - checkAngle) > GM_NULL_TOLERANCE) {
logMessage(_T("\tERROR - XY angle computation not working\n"));
numErr++;
}
else {
logMessage(_T("\tOK - XY angle computation working\n"));
}
return numErr;
}
