//.......................................................................................
void StateMain::Update(float _t)
{
pointsInterpolation[0] = LERP3(_t, linesInterpolation[0]);
pointsInterpolation[1] = LERP3(_t, linesInterpolation[1]);
pointsInterpolation[2] = LERP3(_t, linesInterpolation[2]);
linesInterpolation[3] = line3(pointsInterpolation[0], pointsInterpolation[1]);
linesInterpolation[4] = line3(pointsInterpolation[1], pointsInterpolation[2]);
pointsInterpolation[3] = LERP3(_t, linesInterpolation[3]);
pointsInterpolation[4] = LERP3(_t, linesInterpolation[4]);
linesInterpolation[5] = line3(pointsInterpolation[3], pointsInterpolation[4]);
pointsInterpolation[5] = LERP3(_t, linesInterpolation[5]);
// update buffers
glBindVertexArray(vaoLines);
glBindBuffer(GL_ARRAY_BUFFER, vboLines);
glBufferData(GL_ARRAY_BUFFER, sizeof(line3) * nLines, linesInterpolation, GL_DYNAMIC_DRAW);
glBindVertexArray(vaoPoints);
glBindBuffer(GL_ARRAY_BUFFER, vboPoints);
glBufferData(GL_ARRAY_BUFFER, sizeof(Vector3f) * nLines, pointsInterpolation, GL_DYNAMIC_DRAW);
glBindVertexArray(0);
} // end StateMain::Update()
void dump_grid_range_highlight(const Grid & grid,
ssize_t ix0, ssize_t iy0,
ssize_t ix1, ssize_t iy1,
ssize_t ixhigh, ssize_t iyhigh,
FILE * stream)
{
PVDEBUG("%zu %zu %zu %zu %zu %zu\n",
ix0, iy0, ix1, iy1, ixhigh, iyhigh);
fprintf(stream, " ");
for(ssize_t ix(ix0); ix <= ix1; ++ix)
if(ix == ixhigh) fprintf(stream, " ***********");
else fprintf(stream, " ");
fprintf(stream, " \n");
ssize_t iy(iy1);
for(/**/; iy != iy0; --iy){
const char * high(iy == iyhigh ? "*" : " ");
linesep(grid, stream, ix0, ix1, 0, " ");
line1(grid, stream, iy, ix0, ix1, 0, high);
line2(grid, stream, iy, ix0, ix1, 0, high);
line3(grid, stream, iy, ix0, ix1, 0, high);
line4(grid, stream, iy, ix0, ix1, 0, high);
}
const char * high(iy == iyhigh ? "*" : " ");
linesep(grid, stream, ix0, ix1, 0, " ");
line1(grid, stream, iy, ix0, ix1, 0, high);
line2(grid, stream, iy, ix0, ix1, 0, high);
line3(grid, stream, iy, ix0, ix1, 0, high);
line4(grid, stream, iy, ix0, ix1, 0, high);
linesep(grid, stream, ix0, ix1, 0, " ");
fprintf(stream, " ");
for(ssize_t ix(ix0); ix <= ix1; ++ix)
if(ix == ixhigh) fprintf(stream, " ***********");
else fprintf(stream, " ");
fprintf(stream, " \n");
}
void dump_grid_range(const Grid & grid,
ssize_t ix0, ssize_t iy0, ssize_t ix1, ssize_t iy1,
FILE * stream)
{
PVDEBUG("%zu %zu %zu %zu\n", ix0, iy0, ix1, iy1);
const char * even("");
const char * oddsep;
const char * oddpre;
if (grid.GetNeighborhood() == Grid::SIX) {
oddsep = "+-----";
oddpre = " ";
}
else {
oddsep = even;
oddpre = even;
}
ssize_t iy(iy1);
const char * prefix;
for(/**/; iy != iy0; --iy){
if(iy % 2){
linesep(grid, stream, ix0, ix1, oddsep, 0);
prefix = oddpre;
}
else{
linesep(grid, stream, ix0, ix1, even, 0);
prefix = even;
}
line1(grid, stream, iy, ix0, ix1, prefix, 0);
line2(grid, stream, iy, ix0, ix1, prefix, 0);
line3(grid, stream, iy, ix0, ix1, prefix, 0);
line4(grid, stream, iy, ix0, ix1, prefix, 0);
}
if(iy % 2){
linesep(grid, stream, ix0, ix1, oddsep, 0);
prefix = oddpre;
}
else{
linesep(grid, stream, ix0, ix1, even, 0);
prefix = even;
}
line1(grid, stream, iy, ix0, ix1, prefix, 0);
line2(grid, stream, iy, ix0, ix1, prefix, 0);
line3(grid, stream, iy, ix0, ix1, prefix, 0);
line4(grid, stream, iy, ix0, ix1, prefix, 0);
if(iy % 2)
linesep(grid, stream, ix0, ix1, even, 0);
else
linesep(grid, stream, ix0, ix1, oddsep, 0);
}
Connection *ConnectionManager::getConnection(int x, int y, int &point, QPointF *intersectPnt) {
QPointF from;
QPointF to;
for (ConnectionList::iterator it = m_conns.begin(); it != m_conns.end(); ++it) {
Connection *c = *it;
from = c->points[0];
for (int i = 1; i < c->points.size(); ++i) {
to = c->points[i];
QLineF line(from, to);
QLineF line2(x-5, y-5, x+5, y+5);
if (line.intersect(line2, intersectPnt)==QLineF::BoundedIntersection) {
point = i;
return (*it);
}
else {
QLineF line3(x-5, y+5, x+5, y-5);
if (line.intersect(line3, intersectPnt)==QLineF::BoundedIntersection) {
point = i;
return (*it);
}
}
from = to;
}
}
return 0;
}
void test_circuit_rc()
{
ngdc dc("dc1", 5);
ngresistor r("r1", 5);
ngcapacitor c("c1", 0.2);
ngground gnd;
ngline line1(dc[0], r[0]);
ngline line2(r[1], c[0]);
ngline line3(c[1], dc[1]);
ngline line4(dc[0], gnd[0]);
schema sch("design1");
sch.AddDevice(&dc);
sch.AddDevice(&r);
sch.AddDevice(&c);
sch.AddDevice(&gnd);
sch.AddLine(&line1);
sch.AddLine(&line2);
sch.AddLine(&line3);
sch.AddLine(&line4);
circuit cir(&sch);
cir.Tran("1s");
do
{
Sleep(200);
} while (cir.IsRunning());
}
void Rectangle::draw(Frame* fr) {
/*
* x1y2-------x2y2
* | |
* | |
* | |
* x1y1-------x2y1
*/
if(x1 > x2 && y1 > y2) {
std::swap(x1, x2);
std::swap(y1, y2);
}
if(is_valid(fr)) {
// Create 4 lines
Line line1(x1, y1, x1, y2);
Line line2(x1, y2, x2, y2);
Line line3(x2, y2, x2, y1);
Line line4(x2, y1, x1, y1);
// Draw them
line1.draw(fr);
line2.draw(fr);
line3.draw(fr);
line4.draw(fr);
} else {
throw std::runtime_error("Rechteck nicht korrekt!");
}
}
void tst_QRay3D::contains_ray()
{
QFETCH(QVector3D, origin);
QFETCH(QVector3D, direction);
QFETCH(QVector3D, point);
QFETCH(bool, contains);
Qt3DRender::RayCasting::QRay3D line(origin, direction);
if (contains) {
Qt3DRender::RayCasting::QRay3D line2(point, direction);
QVERIFY(line.contains(line2));
QVERIFY(line2.contains(line));
// Reversed direction is also contained.
Qt3DRender::RayCasting::QRay3D line3(point, -direction);
QVERIFY(line.contains(line2));
QVERIFY(line2.contains(line));
// Different direction.
Qt3DRender::RayCasting::QRay3D line4(point, QVector3D(direction.y(), direction.x(), direction.z()));
QVERIFY(!line.contains(line4));
QVERIFY(!line4.contains(line));
} else {
Qt3DRender::RayCasting::QRay3D line2(point, direction);
QVERIFY(!line.contains(line2));
QVERIFY(!line2.contains(line));
}
}
vector<Pixel> Rectangle::getPixels() const{
Point p1(mPositionX,mPositionY);
Point p2(mPositionX+mWidth,mPositionY);
Point p3(mPositionX,mPositionY+mHeight);
Point p4(mPositionX+mWidth,mPositionY+mHeight);
Line line1(p1,p2,mColor);
Line line2(p2,p4,mColor);
Line line3(p3,p4,mColor);
Line line4(p1,p3,mColor);
vector<Pixel> linePixels1 = line1.getPixels();
vector<Pixel> linePixels2 = line2.getPixels();
vector<Pixel> linePixels3 = line3.getPixels();
vector<Pixel> linePixels4 = line4.getPixels();
vector<Pixel> pixels;
pixels.insert(pixels.end(),linePixels1.begin(),linePixels1.end());
pixels.insert(pixels.end(),linePixels2.begin(),linePixels2.end());
pixels.insert(pixels.end(),linePixels3.begin(),linePixels3.end());
pixels.insert(pixels.end(),linePixels4.begin(),linePixels4.end());
return pixels;
}
Label TemplatePrinter::templateFiller(const char * templateFilePath)
{
/* THIS IMPLEMENTATION IS TEMPORARY
* In the future, TemplatePrinter will have a container of fields and columns
* just like the label. The templateFiller will loop through the fields and columns to
* A) format the label object with enough columns and fields and B) fill in the data from
* the text fields of the gui. DO NOT rely on this implementation in the future!
*/
Label tempLabel;
QFile xmlFile(templateFilePath);
LabelParser parser(xmlFile, tempLabel);
parser.start();
QString line1(ui->lineEdit->displayText());
QString line2(ui->lineEdit_2->displayText());
QString line3(ui->lineEdit_3->displayText());
Column* column = &((tempLabel.getColumns())->at(0));
Field* field1 = &(column->fields.at(0));
field1->value = line1;
Field* field2 = &(column->fields.at(1));
field2->value = line2;
Column* column2 = &((tempLabel.getColumns())->at(1));
Field* field3 = &(column2->fields.at(1));
field3->value = line3;
return tempLabel;
}
int crossplane(plane f,line3 &u)//平面和平面的交线
{
point3 oo=o.det(f.o);
point3 v=o.det(oo);
double d=fabs(f.o.dot(v));
if (dblcmp(d)==0)return 0;
point3 q=a.add(v.mul(f.o.dot(f.a.sub(a))/d));
u=line3(q,q.add(oo));
return 1;
}
void PlaneTest::collisionLineSegment() {
Shapes::Plane plane(Vector3(), Vector3::yAxis());
Shapes::LineSegment3D line({0.0f, -0.1f, 0.0f}, {0.0f, 7.0f, 0.0f});
Shapes::LineSegment3D line2({0.0f, 0.1f, 0.0f}, {0.0f, 7.0f, 0.0f});
Shapes::LineSegment3D line3({0.0f, -7.0f, 0.0f}, {0.0f, -0.1f, 0.0f});
VERIFY_COLLIDES(plane, line);
VERIFY_NOT_COLLIDES(plane, line2);
VERIFY_NOT_COLLIDES(plane, line3);
}
void
MFD::setPage ( MfdPage page )
{
std::string line1(page.getLine1());
std::string line2(page.getLine2());
std::string line3(page.getLine3());
if(isUpdateEnabled) {
MFD::setLine1(line1, true);
MFD::setLine2(line2, true);
MFD::setLine3(line3, true);
}
}
main(){
data_init();
line1(1,1,17,13);
data_print();
data_init();
line3(1,1,17,11);
line3(1,4,7,18);
data_print();
data_init();
line4(7,5,5,1);
line4(5,5,1,3);
line4(18,1,2,11);
line4(18,4,6,18);
data_print();
data_init();
line5(5,5,18,9, 3);
line5(10,18,3,9, 4);
data_print();
data_init();
circ2(12,12,3);
circ2(10,10,9);
data_print();
data_init();
circ3(11,10,3, 1,1);
circ3(10,10,9, 3,0);
data_print();
data_init();
circ3(10,10,8, 8,0);
data_print();
return 0;
}
void PlaneTest::collisionLineSegment() {
Physics::Plane plane(Vector3(), Vector3::yAxis());
Physics::LineSegment3D line({0.0f, -0.1f, 0.0f}, {0.0f, 7.0f, 0.0f});
Physics::LineSegment3D line2({0.0f, 0.1f, 0.0f}, {0.0f, 7.0f, 0.0f});
Physics::LineSegment3D line3({0.0f, -7.0f, 0.0f}, {0.0f, -0.1f, 0.0f});
randomTransformation(plane);
randomTransformation(line);
randomTransformation(line2);
randomTransformation(line3);
VERIFY_COLLIDES(plane, line);
VERIFY_NOT_COLLIDES(plane, line2);
VERIFY_NOT_COLLIDES(plane, line3);
}
void test_circuit_rc_tran()
{
ngdc dc("dc1", 5);
ngspdt spdt("spdt");
ngresistor r("r1", 5);
ngcapacitor c("c1", 0.2);
ngground gnd;
ngline line1(dc.pos, spdt.throw1);
ngline line2(spdt.pole, r.p1);
ngline line3(r.p2, c.p1);
ngline line4(c.p2, dc.neg);
ngline line5(spdt.throw2, gnd.ground);
ngline line0(dc.neg, gnd.ground);
schema sch;
sch.AddDevices(&dc, &spdt, &r, &c, &gnd, 0);
sch.AddLines(&line1, &line2, &line3, &line4, &line5, &line0, 0);
circuit cir(&sch);
cir.Tran("1t", "100u");
do
{
Sleep(200);
char ch = getchar();
switch (ch)
{
case 'a':
// switchover to charge or discharge
cir.SwitchOver(&spdt);
Sleep(200);
break;
case 'q':
cir.Halt();
default:
break;
};
} while (cir.IsRunning());
cir.Do("plot all");
}
bool Line::visit_collides(const Rect *rect) const
{
Line line1(rect->get_x(), rect->get_y(),
rect->get_x() + rect->get_w() - 1,
rect->get_y());
Line line2(rect->get_x(), rect->get_y(),
rect->get_x(),
rect->get_y() + rect->get_h() - 1);
Line line3(rect->get_x() + rect->get_w() - 1,
rect->get_y(),
rect->get_x() + rect->get_w() - 1,
rect->get_y() + rect->get_h() - 1);
Line line4(rect->get_x(),
rect->get_y() + rect->get_h() - 1,
rect->get_x() + rect->get_w() - 1,
rect->get_y() + rect->get_h() - 1);
return rect->contains(m_x1, m_y1) ||
rect->contains(m_x2, m_y2) ||
visit_collides(&line1) || visit_collides(&line2) ||
visit_collides(&line3) || visit_collides(&line4);
}
int main(int argc, char *argv[]) {
int maxX = 0;
int maxY = 0;
int minX = 0;
int minY = 0;
// Initialize lines vector with "random" numbers
std::vector<Lines<int> > lines;
Lines<int> line1(-533445, 535435, -23424, 312312);
Lines<int> line2(343242, 223, 0, -42);
Lines<int> line3(211, 25454, 44674, 1665432);
Lines<int> line4(-142421, 4256, 98765432, 7653);
lines.push_back(line1);
lines.push_back(line2);
lines.push_back(line3);
lines.push_back(line4);
// Normalize lines between -1 and 1 floats
const std::vector<Lines<float> > normalizedLines = Algorithms::normalizeLines(lines, minX, maxX, minY, maxY);
for(std::vector<Lines<float> >::const_iterator it = normalizedLines.begin(); it != normalizedLines.end(); ++it) {
if (it->x1 > 1 || it->x1 < -1) {
std::cout << "X1 value was out of bounds";
return -1;
} else if (it->x2 > 1 || it->x2 < -1) {
std::cout << "X2 value was out of bounds";
return -1;
} else if (it->y1 > 1 || it->y1 < -1) {
std::cout << "Y1 value was out of bounds";
return -1;
} else if (it->y2 > 1 || it->y2 < -1) {
std::cout << "Y2 value was out of bounds";
return -1;
}
}
std::cout << "SUCCESSSSS!!";
return 0;
}
/* same effect to test_rc_charge_discharge()
title rc charge discharge
.global gnd
vdc1 1 0 dc 5.000e+000
xspdt 1 2 0 spdt
rr1 2 3 5.000e+000
cc1 3 0 2.000e-001
.subckt spdt 1 2 3 params: vstatus=0 ron=1e-8 roff=1e30
r1 0 6 20
v1 6 0 dc {vstatus}
w0 2 1 v1 nc_contact
w1 2 3 v1 no_contact
.model no_contact csw (it=0.05 ih=0.025 ron={ron} roff={roff})
.model nc_contact csw (it=0.05 ih=0.025 ron={roff} roff={ron})
.ends
.control
stop when time = 5s
tran 100u 10s uic
alter v.xspdt.v1=-2
resume
plot all
.endc
*/
void test_rc_charge_discharge()
{
ngdc dc("dc1", 5);
ngspdt spdt("spdt");
ngresistor r("r1", 5);
ngcapacitor c("c1", 0.2);
ngground gnd;
ngline line1(dc.pos, spdt.throw1);
ngline line2(spdt.pole, r.p1);
ngline line3(r.p2, c.p1);
ngline line4(c.p2, dc.neg);
ngline line5(spdt.throw2, gnd.ground);
ngline line0(dc.neg, gnd.ground);
schema sch;
sch.AddDevices(&dc, &spdt, &r, &c, &gnd, 0);
sch.AddLines(&line1, &line2, &line3, &line4, &line5, &line0, 0);
circuit cir(&sch);
// transient analysis last 10 seconds
cir.Tran("10", "1m");
do
{
Sleep(100);
//charge 5 seconds. when time passes 5 seconds, start to discharge.
static bool done = false;
if (cir.CurrentValue("time") >= 5 && !done)
{
cir.SwitchOver(&spdt);
Sleep(200);
done = true;
}
} while (cir.IsRunning());
cir.Do("plot all");
getchar();
}
void ossimFeatherMosaic::ossimFeatherInputInformation::setVertexList(const std::vector<ossimIpt>& validVertices)
{
const char* MODULE = "ossimFeatherMosaic::ossimFeatherInputInformation::setVertexList()";
theValidVertices = validVertices;
theCenter = ossimDpt(0,0);
theAxis1 = ossimDpt(1, 0);
theAxis2 = ossimDpt(0, 1);
theAxis1Length = 1;
theAxis2Length = 1;
double xSum=0.0, ySum=0.0;
ossim_uint32 upperBound = (ossim_uint32)validVertices.size();
if(upperBound)
{
for(ossim_uint32 index = 0; index < upperBound; ++index)
{
xSum += validVertices[index].x;
ySum += validVertices[index].y;
}
theCenter.x = xSum/upperBound;
theCenter.y = ySum/upperBound;
// for now we just want a quick implementation of something
// and we know that we have 4 vertices for the bounding valid
// vertices.
//
if(upperBound == 4)
{
ossimDpt edgeDirection1 = validVertices[1] - validVertices[0];
ossimDpt edgeDirection2 = validVertices[2] - validVertices[1];
theAxis1 = ossimDpt(-edgeDirection1.y, edgeDirection1.x);
theAxis2 = ossimDpt(-edgeDirection2.y, edgeDirection2.x);
theAxis1 = theAxis1/theAxis1.length();
theAxis2 = theAxis2/theAxis2.length();
ossimLine line1(theCenter,
theCenter + theAxis1*2);
ossimLine line2(validVertices[1],
validVertices[0]);
ossimLine line3(theCenter,
theCenter + theAxis2*2);
ossimLine line4(validVertices[2],
validVertices[1]);
ossimDpt intersectionPoint1 = line1.intersectInfinite(line2);
ossimDpt intersectionPoint2 = line3.intersectInfinite(line4);
theAxis1Length = ossim::round<int>((theCenter-intersectionPoint1).length());
theAxis2Length = ossim::round<int>((theCenter-intersectionPoint2).length());
if(traceDebug())
{
CLOG << "theAxis1Length: " << theAxis1Length << endl
<< "theAxis2Length: " << theAxis2Length << endl
<< "center: " << theCenter << endl;
}
}
}
}
QuadTreeNodeData::REGION_INTERSECTION_FLAG QuadTree::region_intersection(const iterator_base & it, const CG_Region & region )
{
RS_Hatch * hatch = region.hatch;
if(!hatch->hasHole())
return region_intersection(it, region.contour_points );
const RS_Vector * _p_tr = it->tr();
const RS_Vector * _p_tl = it->tl();
const RS_Vector * _p_br = it->br();
const RS_Vector * _p_bl = it->bl();
RS_Line line1(0, RS_LineData(*_p_bl, *_p_br));
if(hatch->hasIntersectionWithEdge(&line1)) return QuadTreeNodeData::INTERSECTION_REGION;
RS_Line line2(0, RS_LineData(*_p_br, *_p_tr));
if(hatch->hasIntersectionWithEdge(&line2)) return QuadTreeNodeData::INTERSECTION_REGION;
RS_Line line3(0, RS_LineData(*_p_tl, *_p_tr));
if(hatch->hasIntersectionWithEdge(&line3)) return QuadTreeNodeData::INTERSECTION_REGION;
RS_Line line4(0, RS_LineData(*_p_bl, *_p_tl));
if(hatch->hasIntersectionWithEdge(&line4)) return QuadTreeNodeData::INTERSECTION_REGION;
bool this_point_on_hatch;
if(RS_Information::isPointInsideContour(*_p_bl, hatch, &this_point_on_hatch))
return QuadTreeNodeData::IN_REGION;
if(it->has_point(hatch->getData().internal_point))
return QuadTreeNodeData::COVER_REGION;
return QuadTreeNodeData::OUT_REGION;
/*
unsigned int n_point_in_region = 0;
bool has_point_on_region=false;
{
bool this_point_on_hatch;
n_point_in_region += RS_Information::isPointInsideContour(*_p_tr, hatch, &this_point_on_hatch);
if(this_point_on_hatch) has_point_on_region=true;
n_point_in_region += RS_Information::isPointInsideContour(*_p_tl, hatch, &this_point_on_hatch);
if(this_point_on_hatch) has_point_on_region=true;
n_point_in_region += RS_Information::isPointInsideContour(*_p_br, hatch, &this_point_on_hatch);
if(this_point_on_hatch) has_point_on_region=true;
n_point_in_region += RS_Information::isPointInsideContour(*_p_bl, hatch, &this_point_on_hatch);
if(this_point_on_hatch) has_point_on_region=true;
}
std::cout<<n_point_in_region<<std::endl;
if(has_point_on_region)
{
std::cout<<"INTERSECTION_REGION"<<std::endl;
return QuadTreeNodeData::INTERSECTION_REGION;
}
if(n_point_in_region==4)
{
return QuadTreeNodeData::IN_REGION;
}
if(n_point_in_region==0)
{
if(it->has_point(hatch->getData().internal_point))
return QuadTreeNodeData::COVER_REGION;
else return QuadTreeNodeData::OUT_REGION;
}
if(n_point_in_region>0 && n_point_in_region<4)
return QuadTreeNodeData::INTERSECTION_REGION;
*/
}
point3 pttoplane(point3 p)//点到平面最近点
{
line3 u=line3(p,p.add(o));
crossline(u,p);
return p;
}
void DisparityProc::imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_8UC1); // Choose 8 BIT Format, e.g. TYPE_8UC1, MONO8, ...
}
catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
/* --------------------------------------------------------------------------------------------
Select ROI & Inititialize Masks
-------------------------------------------------------------------------------------------*/
cv::Mat image = cv_ptr->image;
cv::Mat ROI(image, cv::Rect(x_min, y_min, width+1, height+1));
cv::Mat X = cv::Mat::zeros(height+1, width+1, CV_32FC1);
cv::Mat Y = cv::Mat::zeros(height+1, width+1, CV_32FC1);
// Create Meshgrid
cv::Range x_bound(0, width);
cv::Range y_bound(0, height);
meshgrid(x_bound, y_bound, X, Y);
// Initialize and compute Masks
const float PI = 3.14159265358979f;
Line line1(-2.3, 175, 2); // Bicaval
Line line2(0.8, 20, 1); // Short Axis
Line line3(0, 60, 1); // 4 Chamber
Ellipse FO(50, 50, 35, 33, -50 * PI/180); // Constructor(x0, y0, a, b, phi)
Rectangle needle(0, 0 ,10, 10); // Constructor(x0, y0, width, height)
cv::Mat mask_line1;
cv::Mat mask_line2;
cv::Mat mask_line3;
cv::Mat mask_FO;
cv::Mat mask_needle;
//std::cerr << "X: " << X.rows << "x" << X.cols << std::endl;
//std::cerr << "Y: " << Y.rows << "x" << Y.cols << std::endl;
line1.calc_mask(mask_line1, X, Y);
line2.calc_mask(mask_line2, X, Y);
line3.calc_mask(mask_line3, X, Y);
FO.calc_mask(mask_FO, X, Y);
// Auto Configuration
// ROI = compute_config(ROI);
/* --------------------------------------------------------------------------------------------
Compute Start & End Points of Cuts
-------------------------------------------------------------------------------------------*/
std::pair<int,int> bicaval_start;
std::pair<int,int> bicaval_stop;
std::pair<int,int> short_axis_start;
std::pair<int,int> short_axis_stop;
std::pair<int,int> four_chamber_start;
std::pair<int,int> four_chamber_stop;
start_end_coord(line1, mask_FO, X, Y, bicaval_start, bicaval_stop);
start_end_coord(line2, mask_FO, X, Y, short_axis_start, short_axis_stop);
start_end_coord(line3, mask_FO, X, Y, four_chamber_start, four_chamber_stop);
/* --------------------------------------------------------------------------------------------
Compute Update of Tenting Curve
Main Idea: Minimize difference between SPOT & OLD SPOT and introduce some kind of spacial delay
-------------------------------------------------------------------------------------------*/
// Check whether or not tenting even occurs
bool th_exceeded = tenting_ocurrs(ROI, mask_FO);
// Update Spot Location if Threshold is exceeded
if (th_exceeded) {
// Estimate current position of needle
find_needle_tip(ROI, mask_FO, needle);
needle_tip_ = needle.get_mid();
if (old_spot_init) {
// Compute whether or not intersection occurs - for each Projection
std::pair<int,int> tmp_proj1 = line1.projection(needle_tip_, bicaval_start);
std::pair<int,int> tmp_proj2 = line2.projection(needle_tip_, short_axis_start);
std::pair<int,int> tmp_proj3 = line3.projection(needle_tip_, four_chamber_start);
bool moving1 = needle_moving(old_proj_bicaval, tmp_proj1);
bool moving2 = needle_moving(old_proj_short_axis, tmp_proj2);
bool moving3 = needle_moving(old_proj_four_chamber, tmp_proj3);
// Update Bicaval
if (!moving1) {
proj_bicaval = old_proj_bicaval; // Stay Still
}
else {
minimize_with_constraint(needle, line1, 0); // Minimize Scalar Product
}
// Update Short Axis
if (!moving2) {
proj_short_axis = old_proj_short_axis; // Stay Still
}
else {
minimize_with_constraint(needle, line2, 1); // Minimize Scalar Product
}
// Update 4 Chamber
if (!moving3) {
proj_four_chamber = old_proj_four_chamber; // Stay Still
}
else {
minimize_with_constraint(needle, line3, 2); // Minimize Scalar Product
}
}
else {
// Old Spot not initialized -> Initialize
initialize_spot(needle,line1, bicaval_start, 0);
initialize_spot(needle,line2, short_axis_start, 1);
initialize_spot(needle,line3, four_chamber_start, 2);
old_spot_init = true;
}
}
else {
//std::cerr << "Threshold NOT exceeded..." << std::endl;
// Draw straight line
proj_bicaval = bicaval_start;
proj_short_axis = short_axis_start;
proj_four_chamber = four_chamber_start;
old_spot_init = false;
}
// Derive x/y _ points (new 1D coord sys) from spot
derive_xy_points(bicaval_start, bicaval_stop, ROI, 0);
derive_xy_points(short_axis_start, short_axis_stop, ROI, 1);
derive_xy_points(four_chamber_start, four_chamber_stop, ROI, 2);
// Update Old Spot
old_proj_bicaval = line1.projection(proj_bicaval, bicaval_start);
old_proj_short_axis = line2.projection(proj_short_axis, short_axis_start);
old_proj_four_chamber = line3.projection(proj_four_chamber, four_chamber_start);
// Print Values of Needle Location
/*
std::cerr << "Needle Location:" << std::endl;
int x_print = needle_tip_.first;
int y_print = needle_tip_.second;
int value = ROI.at<uint8_t>(y_print, x_print);
std::cerr << "(X,Y) = (" << x_print << ", " << y_print << ") : " << value << std::endl;
*/
// For Debugging -> Draw on FO
ROI = draw_on_FO (
ROI,
mask_FO,
mask_line1,
mask_line2,
mask_line3,
bicaval_start,
bicaval_stop,
short_axis_start,
short_axis_stop,
four_chamber_start,
four_chamber_stop,
needle );
/* --------------------------------------------------------------------------------------------
Interpoplate the two parabolas & evaluate on a fine mesh
-------------------------------------------------------------------------------------------*/
/*
* depricated...
*
Parabola left(x_points[0], y_points[0], x_points[1], y_points[1]);
Parabola right(x_points[2], y_points[2], x_points[1], y_points[1]);
int N_vis = 1000;
std::vector<float> x_vis(N_vis, 0);
std::vector<float> y_vis(N_vis, 0);
float h = (x_points[2]-x_points[0])/(N_vis-1);
for(unsigned int i=0; i<x_vis.size(); ++i) {
x_vis[i] = x_points[0] + i*h;
if (x_vis[i] < x_points[1])
y_vis[i] = left.eval_para(x_vis[i]);
else
y_vis[i] = right.eval_para(x_vis[i]);
}
*/
/* --------------------------------------------------------------------------------------------
Create Ros - Messages and publish data
-------------------------------------------------------------------------------------------*/
// Write ROI to published image
cv_ptr->image = ROI;
// Build Messages - BICAVAL
std_msgs::Float64MultiArray bicaval_x;
std_msgs::Float64MultiArray bicaval_y;
bicaval_x.layout.dim.push_back(std_msgs::MultiArrayDimension());
bicaval_x.layout.dim[0].size = x_pt_bicaval.size();
bicaval_x.layout.dim[0].stride = 1;
bicaval_x.layout.dim[0].label = "Bicaval_X_Val";
bicaval_x.data.clear();
bicaval_x.data.insert(bicaval_x.data.end(), x_pt_bicaval.begin(), x_pt_bicaval.end());
bicaval_y.layout.dim.push_back(std_msgs::MultiArrayDimension());
bicaval_y.layout.dim[0].size = y_pt_bicaval.size();
bicaval_y.layout.dim[0].stride = 1;
bicaval_y.layout.dim[0].label = "Bicaval_Y_Val";
bicaval_y.data.clear();
bicaval_y.data.insert(bicaval_y.data.end(), y_pt_bicaval.begin(), y_pt_bicaval.end());
// Build Messages - SHORT AXIS
std_msgs::Float64MultiArray short_axis_x;
std_msgs::Float64MultiArray short_axis_y;
short_axis_x.layout.dim.push_back(std_msgs::MultiArrayDimension());
short_axis_x.layout.dim[0].size = x_pt_short_axis.size();
short_axis_x.layout.dim[0].stride = 1;
short_axis_x.layout.dim[0].label = "Short_Axis_X_Val";
short_axis_x.data.clear();
short_axis_x.data.insert(short_axis_x.data.end(), x_pt_short_axis.begin(), x_pt_short_axis.end());
short_axis_y.layout.dim.push_back(std_msgs::MultiArrayDimension());
short_axis_y.layout.dim[0].size = y_pt_short_axis.size();
short_axis_y.layout.dim[0].stride = 1;
short_axis_y.layout.dim[0].label = "Short_Axis_Y_Val";
short_axis_y.data.clear();
short_axis_y.data.insert(short_axis_y.data.end(), y_pt_short_axis.begin(), y_pt_short_axis.end());
// Build Messages - 4 CHAMBER
std_msgs::Float64MultiArray four_chamber_x;
std_msgs::Float64MultiArray four_chamber_y;
four_chamber_x.layout.dim.push_back(std_msgs::MultiArrayDimension());
four_chamber_x.layout.dim[0].size = x_pt_four_chamber.size();
four_chamber_x.layout.dim[0].stride = 1;
four_chamber_x.layout.dim[0].label = "Four_Chamber_X_Val";
four_chamber_x.data.clear();
four_chamber_x.data.insert(four_chamber_x.data.end(), x_pt_four_chamber.begin(), x_pt_four_chamber.end());
four_chamber_y.layout.dim.push_back(std_msgs::MultiArrayDimension());
four_chamber_y.layout.dim[0].size = y_pt_four_chamber.size();
four_chamber_y.layout.dim[0].stride = 1;
four_chamber_y.layout.dim[0].label = "Four_Chamber_Y_Val";
four_chamber_y.data.clear();
four_chamber_y.data.insert(four_chamber_y.data.end(), y_pt_four_chamber.begin(), y_pt_four_chamber.end());
// Build Messages - NEEDLE LOCATION
std_msgs::Float64MultiArray needle_loc_msg;
bool punctured = true;
std::pair<float,float> tmp_ = FO.map_to_unit_circle(needle_tip_);
tmp_.second = -tmp_.second;
needle_loc_msg.layout.dim.push_back(std_msgs::MultiArrayDimension());
needle_loc_msg.layout.dim[0].size = 3;
needle_loc_msg.layout.dim[0].stride = 1;
needle_loc_msg.layout.dim[0].label = "Needle_Location";
needle_loc_msg.data.clear();
needle_loc_msg.data.push_back(punctured);
needle_loc_msg.data.push_back(tmp_.first);
needle_loc_msg.data.push_back(tmp_.second);
// Publish
image_pub_.publish(cv_ptr->toImageMsg());
bicaval_x_pub.publish(bicaval_x);
bicaval_y_pub.publish(bicaval_y);
short_axis_x_pub.publish(short_axis_x);
short_axis_y_pub.publish(short_axis_y);
four_chamber_x_pub.publish(four_chamber_x);
four_chamber_y_pub.publish(four_chamber_y);
needle_loc_pub.publish(needle_loc_msg);
}
static int read_rawgraphics(void *v, int *nelem,
const molfile_graphics_t **data) {
grasp_t *grasp = (grasp_t *)v;
FILE *infile = grasp->fd;
// Reverse engineering is your friend, and combined with FORTRAN code, voila!
// od -c shows the header starts off:
// \0 \0 \0 P f o r m a t = 2
// and according to ungrasp, this is a 1 for grasp versions 1.0
// and 1.1, and 2 for grasp version 1.2
// Also, the header lines are of length 80 characters + 4 header chars
// + 4 trailer chars
// The 4 bytes at the beginning/end are standard Fortran array trash
/// Pointers grassp type
GRASSP datax;
char trash[4];
#define TRASH fread(trash, 4, 1, infile)
char line[81];
// FIRST LINE OF HEADER; contains format type
TRASH;
fread(line, 1, 80, infile);
// make sure it says 'format='
if (strncmp(line, "format=", 7) != 0) {
printf("graspplugin) First characters of file don't look like a GRASP file\n");
return MOLFILE_ERROR;
}
TRASH;
// next char should be a 0 or 1
char gfiletype = line[7];
if (gfiletype == '1') {
gfiletype = 1;
} else if (gfiletype == '2') {
gfiletype = 2;
} else {
printf("graspplugin) GRASP file is in format %c, but only '1' or '2' is supported\n", gfiletype);
return MOLFILE_ERROR;
}
// SECOND LINE: contains "vertices,accessibles,normals,triangles"
TRASH;
fread(line, 1, 80, infile);
TRASH;
// THIRD LINE: contains (0 or more of)?
// "potentials,curvature,distances,gproperty,g2property,vertexcolor
TRASH;
line3(infile, &datax);/// Reads line 3
TRASH;
// FOURTH LINE stores vertices, triangles, gridsize, lattice spacing
int nvert, ntriangles, gridsize;
float lattice;
TRASH;
fread(line, 1, 80, infile);
TRASH;
sscanf(line, "%d%d%d%f", &nvert, &ntriangles, &gridsize, &lattice);
/// Stores color
float *colores = new float [3*nvert];
// FIFTH LINE stores the center (x,y,z) position
float center[3];
TRASH;
fread(line, 1, 80, infile);
TRASH;
sscanf(line, "%f%f%f", center, center+1, center+2);
float *vertex = new float[3 * nvert];
float *access = new float[3 * nvert];
float *normal = new float[3 * nvert];
int *triangle = new int[3 * ntriangles];
float *properties = new float[3* nvert];
if (!vertex || !access || !normal || !triangle || !properties) {
delete [] vertex;
delete [] access;
delete [] normal;
delete [] triangle;
delete [] properties;
printf("graspplugin) Failed vertex/access/normal/triangle allocations.\n");
return MOLFILE_ERROR;
}
// ungrasp says:
// if (filetype.eq.1) then integer*2
// if (filetype.eq.2) then integer*4
// And read them in. Who needs error checking?
TRASH;
fread(vertex, 3 * sizeof(float), nvert, infile);
TRASH;
TRASH;
fread(access, 3 * sizeof(float), nvert, infile);
TRASH;
TRASH;
fread(normal, 3 * sizeof(float), nvert, infile);
TRASH;
if (is_little_endian()) {
swap4_aligned(vertex, 3*nvert);
swap4_aligned(access, 3*nvert);
swap4_aligned(normal, 3*nvert);
}
if (gfiletype == 2) {
TRASH;
fread(triangle, 3 * sizeof(int), ntriangles, infile);
TRASH;
TRASH;
fread(properties, 3 * sizeof(float), nvert, infile);
if (is_little_endian()) {
swap4_aligned(triangle, 3*ntriangles);
swap4_aligned(properties, 3*nvert);
}
} else {
#if 1
int i;
short *striangle = new short[3 * ntriangles];
if (!striangle) {
delete [] vertex;
delete [] access;
delete [] normal;
delete [] triangle;
delete [] properties;
printf("graspplugin) Failed short triangle allocation.\n");
return MOLFILE_ERROR;
}
TRASH;
fread(striangle, sizeof(short), 3 * ntriangles, infile);
TRASH;
TRASH;
fread(properties, sizeof(float), 3 * nvert, infile);
if (is_little_endian()) {
swap2_aligned(striangle, 3 * ntriangles);
swap4_aligned(properties, 3*nvert);}
for (i=0; i<3*ntriangles; i++) {
triangle[i] = striangle[i];
}
delete [] striangle;
#else
// do it the slow way (converting from short to int)
int i;
short tmp[3];
TRASH;
for (i=0; i<ntriangles; i++) {
fread(tmp, sizeof(short), 3, infile);
if (is_little_endian()) swap2_aligned(tmp, 3);
triangle[3*i+0] = tmp[0];
triangle[3*i+1] = tmp[1];
triangle[3*i+2] = tmp[2];
}
TRASH;
TRASH;
fread(properties, sizeof(float), 3 * nvert, infile);
if (is_little_endian())
swap4_aligned(properties, 3*nvert);
#endif
}
/// Gets properties: potentials, curvature, distances, gproperty, g2property or vertexcolor
Get_Property_Values(&datax, properties, colores, nvert);
// And draw things
grasp->graphics = new molfile_graphics_t[3*ntriangles];
int vert1, vert2, vert3;
for (int tri_count = 0; tri_count < ntriangles; tri_count++) {
vert1 = triangle[3*tri_count+0] - 1; // from 1-based FORTRAN
vert2 = triangle[3*tri_count+1] - 1; // to 0-based C++
vert3 = triangle[3*tri_count+2] - 1;
if (vert1 < 0 || vert2 < 0 || vert3 < 0 ||
vert1 >= nvert || vert2 >= nvert || vert3 >= nvert) {
printf("graspplugin) Error, out-of-range vertex index, aborting.\n");
delete [] vertex;
delete [] access;
delete [] normal;
delete [] triangle;
delete [] properties;
return MOLFILE_ERROR;
}
grasp->graphics[2*tri_count ].type = MOLFILE_TRINORM;
grasp->graphics[2*tri_count+1].type = MOLFILE_NORMS;
grasp->graphics[2*tri_count+2].type = MOLFILE_COLOR;
float *tridata = grasp->graphics[2*tri_count ].data;
float *normdata = grasp->graphics[2*tri_count+1].data;
float *colordata = grasp->graphics[2*tri_count+2].data;
memcpy(tridata , vertex+3*vert1, 3*sizeof(float));
memcpy(tridata+3, vertex+3*vert2, 3*sizeof(float));
memcpy(tridata+6, vertex+3*vert3, 3*sizeof(float));
memcpy(normdata , normal+3*vert1, 3*sizeof(float));
memcpy(normdata+3, normal+3*vert2, 3*sizeof(float));
memcpy(normdata+6, normal+3*vert3, 3*sizeof(float));
memcpy(colordata , properties+3*vert1, 3*sizeof(float));
memcpy(colordata+3, properties+3*vert2, 3*sizeof(float));
memcpy(colordata+6, properties+3*vert3, 3*sizeof(float));
}
*nelem = 2*ntriangles;
*data = grasp->graphics;
delete [] triangle;
delete [] normal;
delete [] access;
delete [] vertex;
delete [] properties;
return MOLFILE_SUCCESS;
}
extern Features::StudioSpline* CreateStudioSplineByLine(Features::AssociativeLine* associativeLine1, Features::AssociativeLine* associativeLine2, Features::AssociativeLine* associativeLine3)
{
Session *theSession = Session::GetSession();
Part *workPart(theSession->Parts()->Work());
NXObject *nullNXObject(NULL);
Features::StudioSplineBuilderEx *studioSplineBuilderEx1;
studioSplineBuilderEx1 = workPart->Features()->CreateStudioSplineBuilderEx(nullNXObject);
// ----------------------------------------------
// Dialog Begin Point
// ----------------------------------------------
Expression *expression1;
expression1 = workPart->Expressions()->CreateSystemExpression("1.000000");
Scalar *scalar1;
scalar1 = workPart->Scalars()->CreateScalarExpression(expression1, Scalar::DimensionalityTypeNone, SmartObject::UpdateOptionWithinModeling);
Line *line1(dynamic_cast<Line *>(associativeLine1->FindObject("CURVE 1")));
Point *point1;
point1 = workPart->Points()->CreatePoint(line1, scalar1, SmartObject::UpdateOptionWithinModeling);
Features::GeometricConstraintData *geometricConstraintData1;
geometricConstraintData1 = studioSplineBuilderEx1->ConstraintManager()->CreateGeometricConstraintData();
geometricConstraintData1->SetPoint(point1);
studioSplineBuilderEx1->ConstraintManager()->Append(geometricConstraintData1);
studioSplineBuilderEx1->Evaluate();
// ----------------------------------------------
// Dialog Begin Point
// ----------------------------------------------
Expression *expression2;
expression2 = workPart->Expressions()->CreateSystemExpression("1.000000");
Scalar *scalar2;
scalar2 = workPart->Scalars()->CreateScalarExpression(expression2, Scalar::DimensionalityTypeNone, SmartObject::UpdateOptionWithinModeling);
Line *line2(dynamic_cast<Line *>(associativeLine2->FindObject("CURVE 1")));
Point *point2;
point2 = workPart->Points()->CreatePoint(line2, scalar2, SmartObject::UpdateOptionWithinModeling);
Features::GeometricConstraintData *geometricConstraintData2;
geometricConstraintData2 = studioSplineBuilderEx1->ConstraintManager()->CreateGeometricConstraintData();
geometricConstraintData2->SetPoint(point2);
studioSplineBuilderEx1->ConstraintManager()->Append(geometricConstraintData2);
studioSplineBuilderEx1->Evaluate();
// ----------------------------------------------
// Dialog Begin Point
// ----------------------------------------------
Expression *expression3;
expression3 = workPart->Expressions()->CreateSystemExpression("1.000000");
Scalar *scalar3;
scalar3 = workPart->Scalars()->CreateScalarExpression(expression1, Scalar::DimensionalityTypeNone, SmartObject::UpdateOptionWithinModeling);
Line *line3(dynamic_cast<Line *>(associativeLine3->FindObject("CURVE 1")));
Point *point3;
point3 = workPart->Points()->CreatePoint(line3, scalar3, SmartObject::UpdateOptionWithinModeling);
Features::GeometricConstraintData *geometricConstraintData3;
geometricConstraintData3 = studioSplineBuilderEx1->ConstraintManager()->CreateGeometricConstraintData();
geometricConstraintData3->SetPoint(point3);
studioSplineBuilderEx1->ConstraintManager()->Append(geometricConstraintData3);
studioSplineBuilderEx1->Evaluate();
NXObject *nXObject;
nXObject = studioSplineBuilderEx1->Commit();
studioSplineBuilderEx1->Destroy();
return (Features::StudioSpline*)nXObject;
}
void KisToolFreehandHelper::paintBezierSegment(KisPaintInformation pi1, KisPaintInformation pi2,
QPointF tangent1, QPointF tangent2)
{
if (tangent1.isNull() || tangent2.isNull()) return;
const qreal maxSanePoint = 1e6;
QPointF controlTarget1;
QPointF controlTarget2;
// Shows the direction in which control points go
QPointF controlDirection1 = pi1.pos() + tangent1;
QPointF controlDirection2 = pi2.pos() - tangent2;
// Lines in the direction of the control points
QLineF line1(pi1.pos(), controlDirection1);
QLineF line2(pi2.pos(), controlDirection2);
// Lines to check whether the control points lay on the opposite
// side of the line
QLineF line3(controlDirection1, controlDirection2);
QLineF line4(pi1.pos(), pi2.pos());
QPointF intersection;
if (line3.intersect(line4, &intersection) == QLineF::BoundedIntersection) {
qreal controlLength = line4.length() / 2;
line1.setLength(controlLength);
line2.setLength(controlLength);
controlTarget1 = line1.p2();
controlTarget2 = line2.p2();
} else {
QLineF::IntersectType type = line1.intersect(line2, &intersection);
if (type == QLineF::NoIntersection ||
intersection.manhattanLength() > maxSanePoint) {
intersection = 0.5 * (pi1.pos() + pi2.pos());
// dbgKrita << "WARINING: there is no intersection point "
// << "in the basic smoothing algoriths";
}
controlTarget1 = intersection;
controlTarget2 = intersection;
}
// shows how near to the controlTarget the value raises
qreal coeff = 0.8;
qreal velocity1 = QLineF(QPointF(), tangent1).length();
qreal velocity2 = QLineF(QPointF(), tangent2).length();
if (velocity1 == 0.0 || velocity2 == 0.0) {
velocity1 = 1e-6;
velocity2 = 1e-6;
warnKrita << "WARNING: Basic Smoothing: Velocity is Zero! Please report a bug:" << ppVar(velocity1) << ppVar(velocity2);
}
qreal similarity = qMin(velocity1/velocity2, velocity2/velocity1);
// the controls should not differ more than 50%
similarity = qMax(similarity, qreal(0.5));
// when the controls are symmetric, their size should be smaller
// to avoid corner-like curves
coeff *= 1 - qMax(qreal(0.0), similarity - qreal(0.8));
Q_ASSERT(coeff > 0);
QPointF control1;
QPointF control2;
if (velocity1 > velocity2) {
control1 = pi1.pos() * (1.0 - coeff) + coeff * controlTarget1;
coeff *= similarity;
control2 = pi2.pos() * (1.0 - coeff) + coeff * controlTarget2;
} else {
control2 = pi2.pos() * (1.0 - coeff) + coeff * controlTarget2;
coeff *= similarity;
control1 = pi1.pos() * (1.0 - coeff) + coeff * controlTarget1;
}
paintBezierCurve(pi1,
control1,
control2,
pi2);
}
