bool CalculateLaylines::checkIntersection( const QString &layerName, const QLineF &heading ) {
QString WKTLine = buildWKTLine( heading.p1(), heading.p2() );
return checkIntersection( layerName, WKTLine);
}
void RayTracer::CalculateShadow(Ray* _ray){
// Assume ray has an intersection point;
//std::cout<<"www"<<std::endl;
for(unsigned int i =0 ; i < m_LightList.size(); i++){
std::vector<Vector4f> visiblesample = m_LightList[i]->getVisibleSamplePoint();
float lightintensity = 0.0;
//std::cout<<visiblesample.size()<<std::endl;
for(unsigned int j = 0; j < visiblesample.size();j++){
Vector4f ptolight = visiblesample[j] - _ray->m_pIntersectionProperties->
m_IntersectionPoint;
//ptolight.Print();
ptolight.Normalize();
Line _testray;
_testray.m_Direction = ptolight;
_testray.m_StartPoint = _ray->m_pIntersectionProperties->m_IntersectionPoint + (float)0.0001 *
_ray->m_pIntersectionProperties->m_PlanarNormal;
bool intersect = checkIntersection(_testray);
if(!intersect){
lightintensity+=1.0;
}
}
lightintensity /= (float)(visiblesample.size());
//std::cout<<lightintensity<<" "<<visiblesample.size()<<std::endl;
_ray->setBlockedLight(i,lightintensity);
}
}
void checkIntersection(const std::string& wkt1,
const std::string& wkt2,
int expectedIntersectionNum,
const std::vector<Coordinate>& intPt,
double distanceTolerance)
// throws ParseException
{
GeomPtr g1(reader.read(wkt1));
GeomPtr g2(reader.read(wkt2));
LineString* l1ptr = dynamic_cast<LineString*>(g1.get());
LineString* l2ptr = dynamic_cast<LineString*>(g2.get());
ensure(0 != l1ptr);
ensure(0 != l2ptr);
LineString& l1 = *l1ptr;
LineString& l2 = *l2ptr;
std::vector<Coordinate> pt;
pt.push_back(l1.getCoordinateN(0));
pt.push_back(l1.getCoordinateN(1));
pt.push_back(l2.getCoordinateN(0));
pt.push_back(l2.getCoordinateN(1));
checkIntersection(pt, expectedIntersectionNum,
intPt, distanceTolerance);
}
bool CalculateLaylines::checkIntersection( const QString &layerName, const QPointF &point ) {
QString WKTPoint = buildWKTPoint( point );
return checkIntersection( layerName, WKTPoint);
}
bool CalculateLaylines::publicCheckIntersection( const QString &layerName, const QPointF &point ) {
QString WKTPoint = buildWKTPoint( point );
bool result = checkIntersection( layerName, WKTPoint);
return result;
}
void checkIntersectionNone(const std::string& wkt1,
const std::string& wkt2)
// throws ParseException
{
GeomPtr g1(reader.read(wkt1));
GeomPtr g2(reader.read(wkt2));
LineString* l1ptr = dynamic_cast<LineString*>(g1.get());
LineString* l2ptr = dynamic_cast<LineString*>(g2.get());
ensure(0 != l1ptr);
ensure(0 != l2ptr);
LineString& l1 = *l1ptr;
LineString& l2 = *l2ptr;
std::vector<Coordinate> pt;
pt.push_back(l1.getCoordinateN(0));
pt.push_back(l1.getCoordinateN(1));
pt.push_back(l2.getCoordinateN(0));
pt.push_back(l2.getCoordinateN(1));
std::vector<Coordinate> intPt;
checkIntersection(pt, 0, intPt, 0);
}
void GDALSpatialLinking::run()
{
if (!properInit) {
DM::Logger(DM::Error) << "GDALSpatialLinking init failed";
this->setStatus(DM::MOD_CHECK_ERROR);
return;
}
//Experimental spatilite interface
if (this->experimental) {
leadingView.createSpatialIndex();
std::stringstream query;
query << "UPDATE " << this->linkViewName << " ";
query << "SET " << leadingViewName << "_id=(SELECT P.ogc_fid FROM " << leadingViewName << " as P ";
query << "WHERE ST_WITHIN( CENTROID(" << linkViewName <<".Geometry), P.Geometry) == 1 AND P.ROWID IN (SELECT ROWID FROM SpatialIndex WHERE f_table_name ='";
query << leadingViewName << "' AND search_frame = " << linkViewName << ".Geometry ORDER BY ROWID) )";
DM::Logger(DM::Debug) << query.str();
leadingView.executeSQL(query.str().c_str());
return;
}
leadingView.resetReading();
OGRFeature * lead_feat = 0;
while (lead_feat = leadingView.getNextFeature()) {
OGRFeature * link_feature = 0;
OGRGeometry * lead_geo = lead_feat->GetGeometryRef();
int id = lead_feat->GetFID();
if (!lead_geo) {
DM::Logger(DM::Warning) << "Lead feature geometry is null";
continue;
}
linkView.resetReading();
linkView.setSpatialFilter(lead_geo);
long counter = 0;
while (link_feature = linkView.getNextFeature()) {
OGRGeometry * geo = link_feature->GetGeometryRef();
if (!geo){
DM::Logger(DM::Warning) << "feature link feature geometry is null";
continue;
}
bool isLinked = false;
if (withCentroid)
isLinked = checkCentroid(geo, lead_geo);
else
isLinked = checkIntersection(geo, lead_geo);
if (!isLinked)
continue;
link_feature->SetField(link_name.c_str(), id);
counter++;
}
if (counter % 100000){
linkView.syncAlteredFeatures();
leadingView.syncReadFeatures();
}
}
}
bool CalculateLaylines::checkIntersection( const QString &layerName, const QLineF &heading, const QPolygonF &rhomboid ) {
QString WKTLine = buildWKTLine( heading.p1(), heading.p2() );
QString WKTPolygon = buildWKTPolygon( rhomboid );
return checkIntersection( layerName, WKTLine, WKTPolygon);
}
void object::test<6>()
{
checkIntersection(
"LINESTRING (305690.0434123494 254176.46578338774, 305601.9999843455 254243.19999846347)",
"LINESTRING (305689.6153764265 254177.33102743194, 305692.4999844298 254171.4999983967)",
1,
"POINT (305690.0434123494 254176.46578338774)",
0);
}
void object::test<9>()
{
checkIntersection(
"LINESTRING (163.81867067 -211.31840378, 165.9174252 -214.1665075)",
"LINESTRING (2.84139601 -57.95412726, 469.59990601 -502.63851732)",
1,
"POINT (163.81867067 -211.31840378)",
0);
}
void object::test<10>()
{
checkIntersection(
"LINESTRING (-58.00593335955 -1.43739086465, -513.86101637525 -457.29247388035)",
"LINESTRING (-215.22279674875 -158.65425425385, -218.1208801283 -160.68343590235)",
1,
"POINT ( -215.22279674875 -158.65425425385 )",
0);
}
void object::test<1>()
{
checkIntersection(
"LINESTRING (588750.7429703881 4518950.493668233, 588748.2060409798 4518933.9452804085)",
"LINESTRING (588745.824857241 4518940.742239175, 588748.2060437313 4518933.9452791475)",
1,
"POINT (588748.2060416829 4518933.945284994)",
0);
}
void object::test<2>()
{
checkIntersection(
"LINESTRING (588743.626135934 4518924.610969561, 588732.2822865889 4518925.4314047815)",
"LINESTRING (588739.1191384895 4518927.235700594, 588731.7854614238 4518924.578370095)",
1,
"POINT (588733.8306132929 4518925.319423238)",
0);
}
bool CalculateLaylines::checkIntersection( const QString &layerName, const QPolygonF &triangle, const QPolygonF &rhomboid ) {
//We'll convert QPointF into String so that we can use it in database queries.
QString WKTTriangle = buildWKTPolygon( triangle );
QString WKTPolygon = buildWKTPolygon( rhomboid );
return checkIntersection( layerName, WKTTriangle, WKTPolygon);
}
bool textureCorrectIndices(float x, float y, int height, int width, float* invals, int id, int* TF, PbrtPoint *TV, int* adj, Vector* result) {
// check if it intersects yourself, cuz if it does, just return false
x=x*1.f/width; y=y*1.f/height;
if (checkIntersection(x, y, TV[TF[3*id]].x, TV[TF[3*id]].y, TV[TF[3*id+1]].x, TV[TF[3*id+1]].y, TV[TF[3*id+2]].x, TV[TF[3*id+2]].y, NULL)) {
return false;
}
float bcs[3];
int j;
for (j=0; j<3; j++) {
// odd one out is 3*j+j
int i0, i1, i0t, i1t;
float x0, y0, x1, y1;
float x0t, y0t, x1t, y1t;
if (j==0) { i0=adj[9*id+3*j+1]; i1=adj[9*id+3*j+2]; i0t=TF[3*id+1]; i1t=TF[3*id+2]; }
else if (j==1) {i0=adj[9*id+3*j+2]; i1=adj[9*id+3*j]; i0t=TF[3*id+2]; i1t=TF[3*id]; }
else if (j==2) {i0=adj[9*id+3*j]; i1=adj[9*id+3*j+1]; i0t=TF[3*id]; i1t=TF[3*id+1]; }
x0=TV[i0].x; x1=TV[i1].x; y0=TV[i0].y; y1=TV[i1].y;
x0t=TV[i0t].x; x1t=TV[i1t].x; y0t=TV[i0t].y; y1t=TV[i1t].y;
float denominv=1./(x0*x0 - 2*x0*x1 + x1*x1 + y0*y0 - 2*y0*y1 + y1*y1);
float a=denominv*((x0 - x1)*x0t+(y0 - y1)*y0t+(x1 - x0)*x1t+(y1 - y0)*y1t);
float b=denominv*((y0 - y1)*x0t+(x1 - x0)*y0t+(y1 - y0)*x1t+(x0 - x1)*y1t);
float c=denominv*((x1*x1 - x0*x1 + y1*y1 - y0*y1)*x0t+(x0*y1 - x1*y0)*y0t+(x0*x0 - x1*x0 + y0*y0 - y1*y0)*x1t+(x1*y0 - x0*y1)*y1t);
float d=denominv*((x1*y0 - x0*y1)*x0t+(x1*x1 - x0*x1 + y1*y1 - y0*y1)*y0t+(x0*y1 - x1*y0)*x1t+(x0*x0 - x1*x0 + y0*y0 - y1*y0)*y1t);
float x_=TV[adj[9*id+3*j+j]].x, y_=TV[adj[9*id+3*j+j]].y;
float x_t=a*x_+b*y_+c;
float y_t=-b*x_+a*y_+d;
if (checkIntersection(x, y, x0t, y0t, x1t, y1t, x_t, y_t, bcs)) {
float xactual=(x0t*bcs[0]+x1t*bcs[1]+x_t*bcs[2])*width;
float yactual=(y0t*bcs[0]+y1t*bcs[1]+y_t*bcs[2])*height;
bilinearInterp(xactual, yactual, height, invals, NULL, result);
return true;
}
}
// if you get here, you're either not in your assigned face, or adjacent faces. Most likely you're very close to a vertex, and technically, you'd need to
// look through faces in a fan around your adjacent vertex, but for now we'll just interpolate as usual
return false;
}
/** checkCollision
* return : the number of collisions
* @position : position of the particle
* @velocity : velocity of the particle
* @newPosition : result position after the collision(s)
* @newVelocity : result velocity after the collision(s)
* @isectPoints : pointer to a vertor of point to store collisions positions
*/
int Border::checkCollision(ofPoint position, ofPoint velocity, ofVec2f* newPosition, ofVec2f* newVelocity, vector<ofPoint>* isectPoints){
bool bCollide = false;
ofPoint isectPoint;
vector<ofPoint> pts = getVertices();
for (int i=0; i<pts.size(); i++) {
// verify if we're moving toward this wall's normal
if(normals[i].dot(velocity) >= 0) continue;
float result = checkIntersection(position, velocity, ofVec2f(pts[i]), edges[i]);
if(result!=-1) {
isectPoint.set(position + velocity * result);
isectPoints->push_back( isectPoint );
// handle slopped wall :
ofVec2f wallVec = edges[i].getNormalized();
ofVec2f wallNormal = wallVec.getPerpendicular();
// get N and T component of speed vector, invert tangential component
float t = wallVec.dot(velocity); // magnitude of the tangential component
float n = wallNormal.dot(velocity); // magnitude of the normal component
// turn into a vector again by multiplying the magnitude with
// the tangential and the (flipped) normal direction
ofVec2f vt = wallVec * t;
ofVec2f vn = wallNormal * -n;
// project <vt, vn> onto x-y axis
float newVx = ofVec2f(1, 0).dot(vn) + ofVec2f(1, 0).dot(vt);
float newVy = ofVec2f(0, 1).dot(vn) + ofVec2f(0, 1).dot(vt);
newVelocity->set(ofPoint(newVx, newVy));
newPosition->set(isectPoint + *(newVelocity) * (1-result));
bCollide = true;
break;
}
}
if(bCollide) {
return 1 + checkCollision(*newPosition, *newVelocity, newPosition, newVelocity, isectPoints);
}
return 0;
}
void object::test<4>()
{
std::vector<Coordinate> intPt;
intPt.push_back(Coordinate(4348437.0557510145, 5552597.375203926));
checkIntersection(
"LINESTRING (4348433.262114629 5552595.478385733, 4348440.849387404 5552599.272022122 )",
"LINESTRING (4348433.26211463 5552595.47838573, 4348440.8493874 5552599.27202212 )",
1,
intPt, 0);
}
void object::test<3>()
{
std::vector<Coordinate> intPt;
intPt.push_back(Coordinate(2089426.5233462777, 1180182.3877339689));
intPt.push_back(Coordinate(2085646.6891757075, 1195618.7333999649));
checkIntersection(
"LINESTRING ( 2089426.5233462777 1180182.3877339689, 2085646.6891757075 1195618.7333999649 )",
"LINESTRING ( 1889281.8148903656 1997547.0560044837, 2259977.3672235999 483675.17050843034 )",
2,
intPt, 0);
}
void object::test<5>()
{
std::vector<Coordinate> pt;
pt.push_back(Coordinate(4348433.262114629, 5552595.478385733));
pt.push_back(Coordinate(4348440.849387404, 5552599.272022122));
pt.push_back(Coordinate(4348433.26211463, 5552595.47838573));
pt.push_back(Coordinate(4348440.8493874, 5552599.27202212));
std::vector<Coordinate> intPt;
intPt.push_back(Coordinate(4348437.0557510145, 5552597.375203926));
checkIntersection( pt, 1, intPt, 0);
}
PassRefPtr<NodeList> SVGSVGElement::collectIntersectionOrEnclosureList(const FloatRect& rect, SVGElement* referenceElement, CollectIntersectionOrEnclosure collect) const
{
Vector<RefPtr<Node> > nodes;
SVGElement* svgElement = Traversal<SVGElement>::firstWithin(referenceElement ? referenceElement : this);
while (svgElement) {
if (collect == CollectIntersectionList) {
if (checkIntersection(svgElement, rect))
nodes.append(svgElement);
} else {
if (checkEnclosure(svgElement, rect))
nodes.append(svgElement);
}
svgElement = Traversal<SVGElement>::next(svgElement, referenceElement ? referenceElement : this);
}
return StaticNodeList::adopt(nodes);
}
PassRefPtr<NodeList> SVGSVGElement::collectIntersectionOrEnclosureList(const FloatRect& rect, SVGElement* referenceElement, CollectIntersectionOrEnclosure collect) const
{
Vector<RefPtr<Node> > nodes;
Element* element = ElementTraversal::next(referenceElement ? referenceElement : this);
while (element) {
if (element->isSVGElement()) {
if (collect == CollectIntersectionList) {
if (checkIntersection(static_cast<SVGElement*>(element), rect))
nodes.append(element);
} else {
if (checkEnclosure(static_cast<SVGElement*>(element), rect))
nodes.append(element);
}
}
element = ElementTraversal::next(element, referenceElement ? referenceElement : this);
}
return StaticNodeList::adopt(nodes);
}
PassRefPtr<NodeList> SVGSVGElement::collectIntersectionOrEnclosureList(const FloatRect& rect, SVGElement* referenceElement, CollectIntersectionOrEnclosure collect) const
{
Vector<RefPtr<Node> > nodes;
Node* node = traverseNextNode(referenceElement ? referenceElement : this);
while (node) {
if (node->isSVGElement()) {
if (collect == CollectIntersectionList) {
if (checkIntersection(static_cast<SVGElement*>(node), rect))
nodes.append(node);
} else {
if (checkEnclosure(static_cast<SVGElement*>(node), rect))
nodes.append(node);
}
}
node = node->traverseNextNode(referenceElement ? referenceElement : this);
}
return StaticNodeList::adopt(nodes);
}
// Main starts from here
int main(int argc, char* argv[]){
if (argc==1){
help();
exit(0);
}
if (argc != 7){
fprintf(stderr,"Wrong amount of parameters!\n");
exit(1);
}
int size_x = atoi(argv[1]); // Are size in centimeters.
int size_y = atoi(argv[2]);
double meters_x = size_x/100; //PPP's density is given in meters.
double meters_y = size_y/100;
int camera_x = atoi(argv[3]); //Camera's position from x=0 in cm.
int good_x = atoi(argv[4]); // Observable item's position from x=0 in cm.
double density = atof(argv[5]); // "lambda", the density given for PPP.
unsigned rounds = atoi(argv[6]); // # of rounds this program is run.
const gsl_rng_type* T; //Random number generator type
T = gsl_rng_default;
r = gsl_rng_alloc(T); //Now r is a pointer to random generator
unsigned hits = 0; // How many times a blocker actually blocs the LOS.
for (unsigned i=0; i<rounds; ++i){
gsl_rng_set(r, random_seed()); //setting random generator seed to random
//Get a number of blockers:
unsigned blockerCount = gsl_ran_poisson(r, density*meters_x*meters_y);
//printf("Result of PPP: %u\n",blockerCount);
Blocker blockers[blockerCount];
generateBlockers(blockers, blockerCount, size_x, size_y, r);
if (checkIntersection(camera_x, good_x, blockers, blockerCount,
size_x, size_y)) { ++hits; }
}
double probs = (double)hits/rounds;
printf("Hits: %u\n",hits);
printf("Rounds: %u\n",rounds);
printf("Propability: %f\n",probs);
gsl_rng_free(r); // Free allocated memory by r
}
void checkIntersection(const std::string& wkt1,
const std::string& wkt2,
int expectedIntersectionNum,
const std::string& expectedWKT,
double distanceTolerance)
//throws ParseException
{
GeomPtr g1(reader.read(wkt1));
GeomPtr g2(reader.read(wkt2));
LineString* l1ptr = dynamic_cast<LineString*>(g1.get());
LineString* l2ptr = dynamic_cast<LineString*>(g2.get());
ensure(0 != l1ptr);
ensure(0 != l2ptr);
LineString& l1 = *l1ptr;
LineString& l2 = *l2ptr;
std::vector<Coordinate> pt;
pt.push_back(l1.getCoordinateN(0));
pt.push_back(l1.getCoordinateN(1));
pt.push_back(l2.getCoordinateN(0));
pt.push_back(l2.getCoordinateN(1));
GeomPtr g(reader.read(expectedWKT));
std::auto_ptr<CoordinateSequence> cs ( g->getCoordinates() );
std::vector<Coordinate> intPt;
for (size_t i=0; i<cs->size(); ++i)
intPt.push_back(cs->getAt(i));
checkIntersection(pt, expectedIntersectionNum,
intPt, distanceTolerance);
}
RGBQUAD TraceOneRay(CRay ray, CSphere S[], CVector3D lightsArray[],int level, CBox sceneBox)
{
int n;
RGBQUAD currentColor;
currentColor.rgbBlue=20;
currentColor.rgbGreen=20;
currentColor.rgbRed=20;
CVector3D hit,light,lightdir,snormal;
if(checkIntersection(ray,S,&hit,&n,sceneBox))
{
currentColor.rgbBlue=0;
currentColor.rgbGreen=0;
currentColor.rgbRed=0;
//Вычисляем нормаль
snormal.x=(hit.x-S[n].center.x);
snormal.y=(hit.y-S[n].center.y);
snormal.z=(hit.z-S[n].center.z);
snormal.NormalizeVector();
//Проверяем каждый источник света
for (int lightIndex=0; lightIndex<LIGHT_COUNT; lightIndex++)
{
light=lightsArray[lightIndex];
lightdir.x=light.x-hit.x;
lightdir.y=light.y-hit.y;
lightdir.z=light.z-hit.z;
lightdir.NormalizeVector();
bool inShadow=false;
//Находим коэффициент освещения
float coefLight=snormal*lightdir;
//Если коэффициент больше 0 то свет попадает
if(coefLight>0){
//луч от источника света к месту пересечения с объектом
CRay lightRay(hit,lightdir);
//Проверяем не затенен ли данный объект другим объектом
for (int index=0; index<SPHERE_COUNT; index++)
{
/*if(index==n)
continue;*/
CVector3D lightHit;
int num;
if (checkIntersection(lightRay,S,&lightHit,&num,sceneBox))
{
inShadow=true;
break;
}
}
if(!inShadow)
{
//Создаем отражающий луч
CVector3D reflectionVector=(ray.vector-2*(((ray.vector*snormal))*snormal));
CRay reflectionRay(hit,reflectionVector);
RGBQUAD reflectionColor;
float coef=1;
for (int i=0; i<level; i++)
{
//coef*=reflectionVector*ray.vector;
coef*=0.5;
}
reflectionColor.rgbBlue=0;
reflectionColor.rgbGreen=0;
reflectionColor.rgbRed=0;
if(level<7)
{
reflectionColor=TraceOneRay(reflectionRay,S,lightsArray,++level,sceneBox);
}
//Свет по Ламберту
if (reflectionColor.rgbRed==20 && reflectionColor.rgbGreen==20 && reflectionColor.rgbBlue==20)
{
reflectionColor.rgbBlue=0;
reflectionColor.rgbGreen=0;
reflectionColor.rgbRed=0;
}
else
{
int k=1;
}
if ((int)currentColor.rgbRed+coefLight*S[n].color.rgbRed*0.5+reflectionColor.rgbRed*coef>=255)
{
currentColor.rgbRed=255;
}
else
{
currentColor.rgbRed+=coefLight*S[n].color.rgbRed*0.5+reflectionColor.rgbRed*coef;
}
if ((int)currentColor.rgbGreen+coefLight*S[n].color.rgbGreen*0.5+reflectionColor.rgbGreen*coef>=255)
{
currentColor.rgbGreen=255;
}
else
{
currentColor.rgbGreen+=coefLight*S[n].color.rgbGreen*0.5+reflectionColor.rgbGreen*coef;
}
if ((int)currentColor.rgbBlue+coefLight*S[n].color.rgbBlue*0.5+reflectionColor.rgbBlue*coef>=255)
{
currentColor.rgbBlue=255;
}
else
{
currentColor.rgbBlue+=coefLight*S[n].color.rgbBlue*0.5+reflectionColor.rgbBlue*coef;
}
//Блинн-Фонг взято с http://www.codermind.com/articles/Raytracer-in-C++-Part-II-Specularity-post-processing.html
float fViewProjection=ray.vector*snormal;
CVector3D blinnVector=lightRay.vector-ray.vector;
float temp=blinnVector*blinnVector;
if (temp!=0)
{
float blinn=1/sqrtSSE(temp) * max(coefLight-fViewProjection,0.0f);
blinn=coef*powf(blinn,20);
if ((int)currentColor.rgbRed+blinn*S[n].color.rgbRed>=255)
{
currentColor.rgbRed=255;
}
else
{
currentColor.rgbRed+=blinn*S[n].color.rgbRed;
}
if ((int)currentColor.rgbGreen+blinn*S[n].color.rgbGreen>=255)
{
currentColor.rgbGreen=255;
}
else
{
currentColor.rgbGreen+=blinn*S[n].color.rgbGreen;
}
if ((int)currentColor.rgbBlue+blinn*S[n].color.rgbBlue>=255)
{
currentColor.rgbBlue=255;
}
else
{
currentColor.rgbBlue+=blinn*S[n].color.rgbBlue;
}
}
}
}
}
}
return currentColor;
}
void CalculateLaylines::getPath( const bool &side, const float &offset, const int &max_turns,
const double &windAngle, const double &layLinesAngle,
const QPointF &boatPos, const QPointF &destinyPos,
const QPolygonF &obstacles_shape, QVector<QPointF> &Path )
{
//Input has to be Geographical data because we check against PostGIS
QPointF TPleft, TPright;
bool sideRight = side;
bool ready, obs_r, obs_l = false;
int count = 0;
// Path is returned by reference, clear:
Path.clear();
// First point in the path is the origin:
Path.append( boatPos);
QPolygonF present_triangle;
QPolygonF last_triangle;
// While the last point in the path is not the destiny
// AND we don't exceed the maximum turning points setting we'll execute following:
while ( Path.last() != destinyPos && count < max_turns ) {
// Clear triangle:
present_triangle.clear();
// Get the turning points between the last point and dest.
// For the first run last point in the path is origin:
UwMath::getTurningPoints( TPleft, TPright,
windAngle, layLinesAngle,
Path.last(), destinyPos);
// Build a new triangle
present_triangle << Path.last();
if (sideRight) present_triangle << TPright;
else present_triangle << TPleft;
present_triangle << destinyPos;
// Check for obstacles in the triangle
obs_r = checkIntersection( "obstacles_r", present_triangle, obstacles_shape );
obs_l = checkIntersection( "obstacles_l", present_triangle, obstacles_shape );
if ( obs_r || obs_l ) {
obstacleFound = true;
// Obstacles found: let's reduce the triangle until they disapear:
ready = false;
last_triangle = present_triangle;
// Reduce triangle to half:
present_triangle = UwMath::triangleToHalf( present_triangle );
while ( !ready ) {
// Check again, put booleans first and checkIntersection won't even be called:
if ( ( obs_r && checkIntersection( "obstacles_r", present_triangle, obstacles_shape ) ) ||
( obs_l && checkIntersection( "obstacles_l", present_triangle, obstacles_shape ) ) ) {
// Still obstacles: reduce again!
last_triangle = present_triangle;
present_triangle = UwMath::triangleToHalf( present_triangle );
} else {
// No more obstacles:
if ( checkOffset( last_triangle, present_triangle, destinyPos, offset ) ) {
// The distance is acceptable, finetune done.
ready = true;
} else {
// We are too far, let's increase the triangle a bit:
present_triangle = UwMath::avgTriangle( present_triangle, last_triangle);
}
}
}
}
// This triangle has no obstacles inside:
Path.append( present_triangle.at( 1 ) );
Path.append( present_triangle.at( 2 ) );
// Add a turn:
count++;
// We continue in the other side:
sideRight = !sideRight;
}
}
QPointF CalculateLaylines::getNextPoint( const QVector<QPointF> &route, const QPointF &boatPos, const float &offset) {
int nearest_point = getNearestPoint(route, boatPos);
if ( checkIntersection( "obstacles_r", boatPos ) ) {
// if we are inside an obstacle, don't even try
return boatPos;
}
// we got the nearest point in the route
// but could be far, let's find out the real
// nearest point of the route by making a
// projection towards it...
// START SEARCH OF PROJECTION FOR FINETUNE
//2. We'll get projections to each point on the route:
QPointF projection_point;
QLineF route_line, projection_line;
QPointF a, b, c;
if ( route.size() >= 2 ) {
// If nearest is the last one:
if ( route.size() - nearest_point == 1 ) {
a = UwMath::toConformal( route.at( nearest_point - 1 ) );
b = UwMath::toConformal( route.at( nearest_point ) );
c = UwMath::toConformal( (const QPointF)boatPos );
projection_point = UwMath::getProjectionPoint( a, b, c);
UwMath::fromConformal( projection_point);
route_line.setP1( route.at( nearest_point - 1 ));
route_line.setP2( route.at( nearest_point ));
projection_line.setP1( boatPos);
projection_line.setP2( projection_point);
// If nearest is the first one:
} else if ( nearest_point == 0 ) {
a = UwMath::toConformal( route.at( nearest_point) );
b = UwMath::toConformal( route.at( nearest_point + 1) );
c = UwMath::toConformal( (const QPointF)boatPos );
projection_point = UwMath::getProjectionPoint( a, b, c);
UwMath::fromConformal( projection_point);
route_line.setP1( route.at( nearest_point));
route_line.setP2( route.at( nearest_point + 1));
projection_line.setP1( boatPos);
projection_line.setP2( projection_point);
// If nearest is not first or last one:
} else {
a = UwMath::toConformal( route.at( nearest_point) );
b = UwMath::toConformal( route.at( nearest_point + 1) );
c = UwMath::toConformal( (const QPointF)boatPos );
projection_point = UwMath::getProjectionPoint( a, b, c);
UwMath::fromConformal( projection_point);
route_line.setP1( route.at( nearest_point));
route_line.setP2( route.at( nearest_point + 1));
projection_line.setP1( boatPos);
projection_line.setP2( projection_point);
// We must check if our position is between nearest and nearest + 1
// or between nearest - 1 and nearest:
if ( !checkIntersection(route_line, projection_line) ) {
a = UwMath::toConformal( route.at( nearest_point - 1 ) );
b = UwMath::toConformal( route.at( nearest_point ) );
c = UwMath::toConformal( (const QPointF)boatPos );
projection_point = UwMath::getProjectionPoint( a, b, c);
UwMath::fromConformal( projection_point);
route_line.setP1( route.at( nearest_point - 1 ));
route_line.setP2( route.at( nearest_point ));
projection_line.setP1( boatPos);
projection_line.setP2( projection_point);
}
}
}
// Next we'll find the checkpoint:
//3. We'll check if there are obstacles in the projeted triangle:
if ( route.size() < 1 ) {
} else if ( route.size() == 1 ) {
// Route to process only has one point:
QLineF heading( boatPos, route.at( nearest_point));
bool hobs_r = checkIntersection( "obstacles_r", heading );
bool hobs_l = checkIntersection( "obstacles_l", heading );
// Finetune checkpoint in the heading:
if ( hobs_r || hobs_l ) {
this->obstacleFound = true;
bool ready = false;
QLineF last_heading;
while ( !ready ) {
if ( ( hobs_r && checkIntersection( "obstacles_r", heading ) ) ||
( hobs_l && checkIntersection( "obstacles_l", heading ) ) ) {
last_heading = heading;
heading = UwMath::lineToHalf( heading);
} else {
if ( checkOffset( heading, last_heading, offset ) )
ready = true;
else
heading = UwMath::avgLine( heading, last_heading);
}
}
//We won't return the point between boat and checkpoint as the next checkpoint on long-term route:
return heading.p2();
} else {
//We won't return the point between boat and checkpoint as the next checkpoint on long-term route:
return heading.p2();
}
} else if ( route.size() > 1 ) {
// Route to process has more than 1 point:
int i = nearest_point;
QPolygonF triangle;
// 1st point:
triangle << boatPos;
// 2nd point:
if ( checkIntersection( route_line, projection_line) ) {
triangle << projection_point;
} else {
triangle << route.at( nearest_point);
}
// 3rd point:
if ( route.at( nearest_point) == route_line.p2() ) {
triangle << route.at( nearest_point);
i = nearest_point - 1;
} else if ( route.at( nearest_point) == route_line.p1() ) {
triangle << route.at( nearest_point + 1);
}
// // ########################################################################
// // SPECIAL CHECK to find obstacles in the heading of the first triangle ##
// // ########################################################################'
if ( triangle.at( 0 ) == boatPos ) {
QLineF heading( triangle.at( 0), triangle.at( 1));
QPolygonF last_triangle;
bool hobs_r = checkIntersection( "obstacles_r", heading, triangle );
bool hobs_l = checkIntersection( "obstacles_l", heading, triangle );
// FINETUNE CHECKPOINT IN THE HEADING
if ( hobs_r || hobs_l ) {
bool ready = false;
QLineF last_heading;
while ( !ready ) {
if ( ( hobs_r && checkIntersection( "obstacles_r", heading, triangle) ) ||
( hobs_l && checkIntersection( "obstacles_l", heading, triangle)) ) {
last_heading = heading;
heading = UwMath::lineToHalf( heading);
last_triangle = triangle;
triangle = UwMath::triangleToHalf(triangle);
} else {
// if ( checkOffset( heading, last_heading, offset ) )
if ( checkOffset_OnePointVersion( last_triangle, triangle, offset ))
ready = true;
else
triangle = UwMath::avgTriangle_OnePointVersion( triangle, last_triangle);
// heading = UwMath::avgLine( heading, last_heading);
}
}
return heading.p2(); //We won't return the point between boat and checkpoint as the next checkpoint on long-term route:
// return triangle.at(2);
}
}
//qDebug() << Q_FUNC_INFO << ": TriangleCount at specialCheck: " << triangleCount;
// ########################################################################
// END SPECIAL CHECK ##
// ########################################################################
bool obs_r = checkIntersection( "obstacles_r", triangle, triangle );
bool obs_l = checkIntersection( "obstacles_l", triangle, triangle );
//Note: This doesn't work correctly. Here the search for the route fails instantly when obstacles
//are found on the route. What if there is obstacle-free route on the next route interval:
while ( !obs_r && !obs_l && i < (route.size()/* - 2*/ )) {
triangle.clear();
triangle << boatPos;
triangle << route.at( i);
triangle << route.at( i + 1);
obs_r = checkIntersection( "obstacles_r", triangle, triangle );
obs_l = checkIntersection( "obstacles_l", triangle, triangle );
// Finetune checkpoint:
if ( obs_r || obs_l ) {
bool ready = false;
QPolygonF last_triangle;
while ( !ready ) {
if ( obs_r && checkIntersection( "obstacles_r", triangle, triangle) ||
obs_l && checkIntersection( "obstacles_l", triangle, triangle) ) {
last_triangle = triangle;
triangle = UwMath::triangleToHalf_OnePointVersion( triangle);
} else {
if (checkOffset_OnePointVersion( last_triangle, triangle, offset ) ){
ready = true;
}
else{
triangle = UwMath::avgTriangle_OnePointVersion( triangle, last_triangle);
}
}
//If the points in the line segment on the side of the route there is no space for the boat in
//the map anymore and we can quit the search:
if(triangle.at(1) == triangle.at(2))
ready = true;
}
}
i++;
}
return triangle.at( 2);
}
// End the process of searching for the checkpoints.
}
void CalculateLaylines::updateLayLines()
{
this->pPolarDiagram->populate();
// LayLines are not calculated with the actual TWA,
// but the TWA that we will have when heading towards our destiny.
//************HARDCODED VALUE FOR futureTrueWindAngle*************
this->trueWindDirection = 270.0;
float futureTrueWindAngle = UwMath::getTWA( geoBoatPos, geoDestinyPos, trueWindDirection );
//************HARDCODED VALUE FOR windSpeed*************
windSpeed = 10;
layLinesAngle = pPolarDiagram->getAngle( windSpeed, futureTrueWindAngle);
// new paths...
pLeftPath = new QVector<QPointF>;
pRightPath = new QVector<QPointF>;
if ( layLinesAngle != 0 ) {
// Here we are not reaching:
QPointF TPleft, TPright;
// Get the turning point without taking care of obstacles:
UwMath::getTurningPoints( TPleft, TPright,
trueWindDirection, layLinesAngle,
geoBoatPos, geoDestinyPos);
// ORDER IS VERY IMPORTANT!!
// This is our area of interest for obstacles checking
QPolygonF rhomboid;
rhomboid << geoBoatPos;
rhomboid << TPleft;
rhomboid << geoDestinyPos;
rhomboid << TPright;
// QLineF heading( geoBoatPos, geoDestinyPos );
// Checking of the heading is made when getting the checkpoint here.
// Next we'll go for obstacles checking:
if ( checkIntersection( "obstacles_r", rhomboid, rhomboid ) ||
checkIntersection( "obstacles_l", rhomboid, rhomboid) ) {
// If we have polygon obstacles in the area
// OR we have line obstacles in the area:
// Populate the right path:
getPath( true, ACCU_OFFSET, MAX_TURNING_POINTS,
trueWindDirection, layLinesAngle,
geoBoatPos, geoDestinyPos, rhomboid, *pRightPath);
// Populate the left path:
getPath( false, ACCU_OFFSET, MAX_TURNING_POINTS,
trueWindDirection, layLinesAngle,
geoBoatPos, geoDestinyPos, rhomboid, *pLeftPath);
} else {
// If we don't have any obstacle in our area
// use master turning points for the path:
// qDebug() << "UPDATELAYLINES NO OBSTACLES";
pRightPath->append( geoBoatPos);
pRightPath->append( TPright );
pRightPath->append( geoDestinyPos);
pLeftPath->append( geoBoatPos);
pLeftPath->append( TPleft );
pLeftPath->append( geoDestinyPos);
}
// End of checking
} else {
// LayLines angle = 0
// Here we are reaching:
pRightPath->append( geoBoatPos);
pRightPath->append( geoDestinyPos);
pLeftPath->append( geoBoatPos);
pLeftPath->append( geoDestinyPos);
}
}
