p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added PointInRectangle functionality

This commit is contained in:
Marc 2018-10-17 14:16:22 +02:00
parent 9718238df6
commit 90443b0152
2 changed files with 60 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
LibCarla/source/carla/geom/Math.h
View File

@ -28,7 +28,7 @@ namespace geom {
return std::min(std::max(a, 0.0), 1.0);
}
template<typename T>
template <typename T>
static T sqr(const T &a) {
return a * a;
}
@ -69,6 +69,26 @@ namespace geom {
const Location projection = v + t * (w - v);
return Distance2D(p, projection);
}
static Location RotatePointOnOrigin2D(Location p, double angle) {
double s = std::sin(angle);
double c = std::cos(angle);
return Location(p.x * c - p.y * s, p.x * s + p.y * c, 0.0);
}
static bool PointInRectangle(
const Location &pos,
const Location &extent,
double angle, // [radians]
const Location &p) {
// Move p relative to pos's position and angle
Location transf_p = RotatePointOnOrigin2D(p - pos, -angle);
if (transf_p.x <= extent.x && transf_p.y <= extent.y &&
transf_p.x >= -extent.x && transf_p.y >= -extent.y) {
return true;
}
return false;
}
};
} // namespace geom

39
LibCarla/source/test/test_geom.cpp
View File

@ -71,3 +71,42 @@ TEST(geom, nearest_point_segment) {
ASSERT_EQ(id, results[i]) << "Fails point number " << i;
}
}
TEST(geom, point_in_rectangle) {
ASSERT_TRUE(Math::PointInRectangle(
Location(0, 0, 0), Location(1, 1, 0), 0, Location(0, 0, 0)));
ASSERT_TRUE(Math::PointInRectangle(
Location(0, 0, 0), Location(1, 1, 0), 0, Location(1, 1, 0)));
ASSERT_TRUE(Math::PointInRectangle(
Location(0, 0, 0), Location(1, 1, 0), 0, Location(-1, 1, 0)));
ASSERT_TRUE(Math::PointInRectangle(
Location(0, 0, 0), Location(1, 1, 0), 0, Location(1, -1, 0)));
ASSERT_TRUE(Math::PointInRectangle(
Location(0, 0, 0), Location(1, 1, 0), 0, Location(-1, -1, 0)));
ASSERT_FALSE(Math::PointInRectangle(
Location(0, 0, 0), Location(1, 1, 0), 0, Location(-1.01, -1.01, 0)));
ASSERT_FALSE(Math::PointInRectangle(
Location(0, 0, 0), Location(1, 1, 0), 0, Location(1.01, 1.01, 0)));
ASSERT_FALSE(Math::PointInRectangle(
Location(1.5, 1.5, 0), Location(1, 1, 0), 0, Location(0, 0, 0)));
ASSERT_TRUE(Math::PointInRectangle(
Location(1.5, 1.5, 0), Location(1, 1, 0), 0, Location(1, 1, 0)));
ASSERT_TRUE(Math::PointInRectangle(
Location(1.5, 1.5, 0), Location(1, 1, 0), 0, Location(2, 1, 0)));
ASSERT_TRUE(Math::PointInRectangle(
Location(1.5, 1.5, 0), Location(1, 1, 0), 0, Location(2, 1, 0)));
ASSERT_TRUE(Math::PointInRectangle(
Location(1.5, 1.5, 0), Location(1, 1, 0), 0, Location(2, 2, 0)));
ASSERT_TRUE(Math::PointInRectangle(
Location(1.5, 1.5, 0), Location(1, 1, 0), 0, Location(1, 1, 0)));
ASSERT_FALSE(Math::PointInRectangle(
Location(0, 0, 0), Location(1, 1, 0), Math::pi_half() * 0.5, Location(1, 1, 0)));
ASSERT_TRUE(Math::PointInRectangle(
Location(0, 0, 0), Location(1, 1, 0), Math::pi_half() * 0.5, Location(1, 0, 0)));
ASSERT_TRUE(Math::PointInRectangle(
Location(0, 2, 0), Location(0.5, 2, 0), Math::pi_half(), Location(2, 2, 0)));
ASSERT_FALSE(Math::PointInRectangle(
Location(0, 2, 0), Location(0.5, 2, 0), Math::pi_half(), Location(2.1, 2, 0)));
ASSERT_FALSE(Math::PointInRectangle(
Location(0, 2, 0), Location(0.5, 2, 0), Math::pi_half(), Location(2, 2.6, 0)));
}