// see if location is past a line perpendicular to
// the line between point1 and point2. If point1 is
// our previous waypoint and point2 is our target waypoint
// then this function returns true if we have flown past
// the target waypoint
bool location_passed_point(struct Location &location,
struct Location &point1,
struct Location &point2)
{
// the 3 points form a triangle. If the angle between lines
// point1->point2 and location->point2 is greater than 90
// degrees then we have passed the waypoint
Vector2f loc1(location.lat, location.lng);
Vector2f pt1(point1.lat, point1.lng);
Vector2f pt2(point2.lat, point2.lng);
float angle = (loc1 - pt2).angle(pt1 - pt2);
if (isinf(angle)) {
// two of the points are co-located.
// If location is equal to point2 then say we have passed the
// waypoint, otherwise say we haven't
if (get_distance(&location, &point2) == 0) {
return true;
}
return false;
} else if (angle == 0) {
// if we are exactly on the line between point1 and
// point2 then we are past the waypoint if the
// distance from location to point1 is greater then
// the distance from point2 to point1
return get_distance(&location, &point1) >
get_distance(&point2, &point1);
}
if (degrees(angle) > 90) {
return true;
}
return false;
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
M113a_Chassis::M113a_Chassis(const std::string& name, bool fixed, ChassisCollisionType chassis_collision_type)
: ChRigidChassis(name, fixed) {
m_inertia.SetElement(0, 0, m_inertiaXX.x());
m_inertia.SetElement(1, 1, m_inertiaXX.y());
m_inertia.SetElement(2, 2, m_inertiaXX.z());
m_inertia.SetElement(0, 1, m_inertiaXY.x());
m_inertia.SetElement(0, 2, m_inertiaXY.y());
m_inertia.SetElement(1, 2, m_inertiaXY.z());
m_inertia.SetElement(1, 0, m_inertiaXY.x());
m_inertia.SetElement(2, 0, m_inertiaXY.y());
m_inertia.SetElement(2, 1, m_inertiaXY.z());
// Belly shape (all dimensions in cm)
// width: 170
// points in x-z transversal plane: (-417.0 -14.3), (4.1, -14.3), (21.4, 34.3)
// thickness: 20
double width = 1.70;
double Ax = -4.17;
double Az = -0.143;
double Bx = 0.041;
double Bz = -0.143;
double Cx = 0.214;
double Cz = 0.343;
double thickness = 0.2;
ChVector<> dims1((Bx - Ax), width, thickness);
ChVector<> loc1(0.5 * (Ax + Bx), 0.0, Az + 0.5 * thickness);
ChQuaternion<> rot1(1, 0, 0, 0);
BoxShape box1(loc1, rot1, dims1);
double alpha = std::atan2(Cz - Bz, Cx - Bx); // pitch angle of front box
ChVector<> dims2((Cx - Bx) / std::cos(alpha), width, thickness);
ChVector<> loc2(0.5 * (Bx + Cx) - 0.5 * thickness * std::sin(alpha), 0.0,
0.5 * (Bz + Cz) + 0.5 * thickness * std::cos(alpha));
ChQuaternion<> rot2 = Q_from_AngY(-alpha);
BoxShape box2(loc2, rot2, dims2);
m_has_primitives = true;
m_vis_boxes.push_back(box1);
m_vis_boxes.push_back(box2);
m_has_mesh = true;
m_vis_mesh_name = "Chassis_POV_geom";
m_vis_mesh_file = "M113/Chassis.obj";
m_has_collision = (chassis_collision_type != ChassisCollisionType::NONE);
switch (chassis_collision_type) {
case ChassisCollisionType::PRIMITIVES:
m_coll_boxes.push_back(box1);
m_coll_boxes.push_back(box2);
break;
case ChassisCollisionType::MESH:
m_coll_mesh_names.push_back("M113/Chassis_Hulls.obj");
break;
default:
break;
}
}
double euclidDistance(const cv::Mat& img1, const cv::Mat& img2, cv::Point2i p1, cv::Point2i p2, int w) {
double dist = 0;
for (int x = 0; x < w; x++) {
for (int y = 0; y < w; y++) {
cv::Point2i loc1(p1.x+x, p1.y+y), loc2(p2.x+x, p2.y+y);
dist += std::pow(img1.at<uchar>(loc1) - img2.at<uchar>(loc2), 2);
}
}
return std::sqrt(dist);
}
void testExtent()
{
QgsRectangle bbox1( 10, 10, 11, 11 ); // out of layer's bounds
QgsPointLocator loc1( mVL, QgsCoordinateReferenceSystem(), &bbox1 );
QgsPointLocator::Match m1 = loc1.nearestVertex( QgsPointXY( 2, 2 ), 999 );
QVERIFY( !m1.isValid() );
QgsRectangle bbox2( 0, 0, 1, 1 ); // in layer's bounds
QgsPointLocator loc2( mVL, QgsCoordinateReferenceSystem(), &bbox2 );
QgsPointLocator::Match m2 = loc2.nearestVertex( QgsPointXY( 2, 2 ), 999 );
QVERIFY( m2.isValid() );
QCOMPARE( m2.point(), QgsPointXY( 1, 1 ) );
}
//cuda version
void LKTracker::normCrossCorrelation(const GpuMat& img1, const GpuMat& img2, const GpuMat& gPoints1, const GpuMat& gPoints2, const vector<Point2f>& points1, const vector<Point2f> points2) {
GpuMat res;
GpuMat rec0;
GpuMat rec1;
similarity.clear();
for (int i = 0; i < points1.size(); i++) {
if (status[i] == 1) {
Rect loc0(points1[i].x, points1[i].y, 10, 10);
Rect loc1(points2[i].x, points2[i].y, 10, 10);
rec0 = GpuMat(img1, loc0);
rec1 = GpuMat(img2, loc1);
gpu::matchTemplate(rec0, rec1, res, CV_TM_CCOEFF_NORMED);
similarity.push_back(((float *)(res.data))[0]);
}
else {
similarity.push_back(0.0);
}
}
rec0.release();
rec1.release();
res.release();
}
