FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

geometry changes/fixes/new features

Browse Source 
new grid walkers + fixes
new test-cases
worked on step/and turn detection
android offline-data-reader
worked on vap-grouping
This commit is contained in:
FrankE
2016-09-07 10:16:51 +02:00
parent a203305628
commit d283d9b326
27 changed files with 976 additions and 333 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
geo/Angle.h
View File

@@ -46,10 +46,13 @@ public:
* - as a change-in-direction between [-PI:+PI]
*/
static float getSignedDiffRAD_2PI(const float r1, const float r2) {
Assert::isBetween(r1, 0.0f, (float)(2*M_PI), "r1 out of bounds");
Assert::isBetween(r2, 0.0f, (float)(2*M_PI), "r2 out of bounds");
const float a1 = (r2-r1);
if (std::abs(a1) < M_PI) {return a1;} else {return (M_PI-a1);}
Assert::isBetween(r1, 0.0f, (float)(2*M_PI), "r1 out of bounds"); // [0:360] deg
Assert::isBetween(r2, 0.0f, (float)(2*M_PI), "r2 out of bounds"); // [0:360] deg
float diff = r1-r2;
if (diff > +M_PI) {diff = -(2*M_PI - diff);}
else if (diff < -M_PI) {diff = +(2*M_PI + diff);}
Assert::isBetween(diff, (float)-M_PI, (float)(+M_PI), "result out of bounds"); // [-180:+180] deg
return diff;
}
/** convert degrees to radians */

60
geo/Line2.h
View File

@@ -55,28 +55,54 @@ public:
bool getSegmentIntersection(const Line2& other) const {
const double delta = 0.0000001;
const float p0_x = p1.x, p1_x = p2.x, p2_x = other.p1.x, p3_x = other.p2.x;
const float p0_y = p1.y, p1_y = p2.y, p2_y = other.p1.y, p3_y = other.p2.y;
const double bx = p2.x - p1.x;
const double by = p2.y - p1.y;
const float s1_x = p1_x - p0_x;
const float s1_y = p1_y - p0_y;
const float s2_x = p3_x - p2_x;
const float s2_y = p3_y - p2_y;
const double dx = other.p2.x - other.p1.x;
const double dy = other.p2.y - other.p1.y;
const float s = (-s1_y * (p0_x - p2_x) + s1_x * (p0_y - p2_y)) / (-s2_x * s1_y + s1_x * s2_y);
const float t = ( s2_x * (p0_y - p2_y) - s2_y * (p0_x - p2_x)) / (-s2_x * s1_y + s1_x * s2_y);
const double b_dot_d_perp = bx*dy - by*dx;
if (s >= 0 && s <= 1 && t >= 0 && t <= 1) {
// Collision detected
// if (i_x != NULL)
// *i_x = p0_x + (t * s1_x);
// if (i_y != NULL)
// *i_y = p0_y + (t * s1_y);
return true;
}
if (std::abs(b_dot_d_perp) == 0) {return false;}
return false; // No collision
const double cx = other.p1.x - p1.x;
const double cy = other.p1.y - p1.y;
const double t = (cx * dy - cy * dx) / b_dot_d_perp;
if(t < 0+delta || t > 1-delta) {return false;}
const double u = (cx * by - cy * bx) / b_dot_d_perp;
if(u < 0+delta || u > 1-delta) {return false;}
// const double delta = 0.0000001;
return true;
// const double bx = p2.x - p1.x;
// const double by = p2.y - p1.y;
// const double dx = other.p2.x - other.p1.x;
// const double dy = other.p2.y - other.p1.y;
// const double b_dot_d_perp = bx*dy - by*dx;
// if (std::abs(b_dot_d_perp) == 0) {return false;}
// const double cx = other.p1.x - p1.x;
// const double cy = other.p1.y - p1.y;
// const double t = (cx * dy - cy * dx) / b_dot_d_perp;
// if(t < 0+delta || t > 1-delta) {
// return false;}
// const double u = (cx * by - cy * bx) / b_dot_d_perp;
// if(u < 0+delta || u > 1-delta) {
// return false;}
// return true;
}
@@ -98,10 +124,12 @@ public:
const float cy = other.p1.y - p1.y;
const float t = (cx * dy - cy * dx) / b_dot_d_perp;
if(t < 0+delta || t > 1-delta) {return false;}
if(t < 0+delta || t > 1-delta) {
return false;}
const float u = (cx * by - cy * bx) / b_dot_d_perp;
if(u < 0+delta || u > 1-delta) {return false;}
if(u < 0+delta || u > 1-delta) {
return false;}
result.x = p1.x + t * bx;
result.y = p1.y + t * by;