bool C3dBoundedLine::planeIntersect(const C3dWorldPoint & n, const double d, C3dWorldPoint & intersection) const
{
double t = (d-n.dot(getStartPoint()))/n.dot(getFinishPoint()-getStartPoint());
if(t>1 || t<0)
return false;
intersection = (getStartPoint() + t*(getFinishPoint()-getStartPoint())).eval();
//cout << intersection.transpose() << " intersects plane at t=" << t << endl;
if(IS_DEBUG) CHECK(!zero(intersection.dot(n) - d), "plane intersect computation failed");
return true;
}
C2dBoundedLine::eLineSide C2dBoundedLine::side(const C2dImagePointPx & point) const
{
C2dImagePointPx bottom, top;
if(getStartPoint().y() > getFinishPoint().y()) {
bottom=getStartPoint();
top=getFinishPoint();
} else {
bottom=getFinishPoint();
top=getStartPoint();
}
C2dImagePointPx pll = bottom-top;
C2dImagePointPx perp(-pll.y(), pll.x());
return (perp.dot(point-bottom) < 0) ? eLeftOfLine : eRightOfLine;
}
int isFinishPointValid(const Track *t)
{
if (NULL == t)
return 0;
const GeoPoint p = getFinishPoint(t);
return isValidPoint(&p);
}
C3dBoundedLine::C3dBoundedLine(const C3dWorldPoint & p1, const C3dWorldPoint & p2) : T3dBoundedLine_base(p1, p2)
{
if(IS_DEBUG)
{
p1.checkInit();
p2.checkInit();
CHECK(getStartPoint() == getFinishPoint() && getStartPoint() != C3dWorldPoint::uninit(), "Line end points are equal");
}
}
optional<const C2dBoundedLine> C3dBoundedLine::projectBounded(const CWorldCamera & P) const
{
const optional<const C2dImagePointPx> p1 = P.projectToPx(getStartPoint());
const optional<const C2dImagePointPx> p2 = P.projectToPx(getFinishPoint());
if(!p1 || !p2)
return optional<const C2dBoundedLine>();
return optional<const C2dBoundedLine>(C2dBoundedLine(*p1,*p2));
}
void C3dBoundedLine::draw(cv::Mat & M, const CWorldCamera & P, const cv::Scalar & col, const int nThickness) const
{
CHECK_MAT_INIT(M);
const optional<const C2dImagePointPx> p0 = P.projectToPx(getStartPoint());
const optional<const C2dImagePointPx> p1 = P.projectToPx(getFinishPoint());
if(p0 && p1)
cv::line(M, *p0, *p1, col, nThickness);
}
GeoPoint getSectorGeoPointAtIndex(const Track *t, int index)
{
if (index < 0) index = 0;
const int max = isStage(t) ? STAGE_SECTOR_COUNT : CIRCUIT_SECTOR_COUNT;
const GeoPoint *sectors = isStage(t) ? t->stage.sectors : t->circuit.sectors;
if (index < max && isValidPoint(sectors + index))
return sectors[index];
// If here, return the finish since that is logically the next sector point.
return getFinishPoint(t);
}
static void setup_geo_circles()
{
const GeoPoint start_p = getStartPoint(g_active_track);
g_geo_circles.start = gc_createGeoCircle(start_p,
g_geo_circle_radius);
const GeoPoint finish_p = getFinishPoint(g_active_track);
g_geo_circles.finish = gc_createGeoCircle(finish_p,
g_geo_circle_radius);
/* Set sector geo circle to first circle */
update_sector_geo_circle(0);
}
void C2dBoundedLine::draw(cv::Mat & M, const int nThickness, const cv::Scalar colour) const
{
//const bool bVerbose = false;
CHECK_MAT_INIT(M);
const C2dImagePointPx p1 = getStartPoint();
const C2dImagePointPx p2 = getFinishPoint();
/*if(!inIm(p1, M, -2000) || !inIm(p1, M, -2000))
{
COUT2("Warning: not drawing 2D line " , *this);
COUT(colour);
return;
}*/
cv::line(M, p1, p2, colour, nThickness);
}
static int isSectorTrackingEnabled(const Track *track)
{
if (!isStartFinishEnabled(track))
return 0;
/*
* Must have >= one valid sector; must start at position 0. Be careful
* when using getSectorGeoPointAtIndex because if a point is in within
* a valid range but is an invalid GPS value, it will return the finish
* line (logical because the last sector boundary is always the finish
* line). So we must compare sector 0 against the finish line and if
* they are the same, then we do not have sector values defined.
*/
const GeoPoint sp0 = getSectorGeoPointAtIndex(track, 0);
const GeoPoint fin = getFinishPoint(track);
return !are_geo_points_equal(&fin, &sp0);
}
/**
* The GeoTriggers used here are effectively software de-bouncing
* systems. They ensure that we don't accidentally trigger an
* event. The tradeoff in using these is that we expect certain
* minimum distances be traveled by the vehicle before debouncing
* happens. In this case we
*/
static void setup_geo_triggers(const Track *track, const float radius)
{
GeoPoint gp;
struct GeoCircle gc;
gp = getStartPoint(track);
gc = gc_createGeoCircle(gp, radius);
g_start_geo_trigger = createGeoTrigger(&gc);
/*
* Trip start geo trigger as unit may get powered up in start
* geo circle
*/
geo_trigger_trip(&g_start_geo_trigger);
gp = getFinishPoint(track);
gc = gc_createGeoCircle(gp, radius);
g_finish_geo_trigger = createGeoTrigger(&gc);
}
C2dLine C2dBoundedLine::unboundedLine() const {
C2dLine lineUnbounded;
lineUnbounded.from2Points(getStartPoint(), getFinishPoint());
return lineUnbounded;
}
const C2dBoundedLine C3dBoundedLine::fastProjectBounded(const CWorldCamera & P) const
{
return C2dBoundedLine(P.fastProject(getStartPoint()), P.fastProject(getFinishPoint()));
}