// Distance to a segment
double FFPoint::distanceToSegment(double& ax, double& ay
, double& bx, double& by){
// Return minimum distance between line segment ab and point
const double ab = (ax-bx)*(ax-bx) + (ay-by)*(ay-by); // i.e. |b-a|^2 - avoid a sqrt
if (ab == 0.0) return distance2D(ax, ay); // a == b case
// Consider the line extending the segment, parameterized as a + t (b - a).
// We find projection of point onto the line.
// It falls where t = [(p-a) . (b-a)] / |b-a|^2
const double t = ((x-ax)*(bx-ax)+(y-ay)*(by-ay))/ab;
if (t < 0.0) return distance2D(ax, ay); // Beyond the 'a' end of the segment
else if (t > 1.0) return distance2D(bx, by); // Beyond the 'b' end of the segment
double px = ax + t*(bx-ax); // Projection falls on the segment
double py = ay + t*(by-ay); // Projection falls on the segment
return distance2D(px, py);
}
float
Duration::parse(const IMC::YoYo* maneuver, Position& last_pos, float last_dur,
std::vector<float>& durations, const SpeedConversion& speed_conv)
{
float speed = convertSpeed(maneuver, speed_conv);
if (speed == 0.0)
return -1.0;
Position pos;
extractPosition(maneuver, pos);
// Use 2D distance here
float horz_dist = distance2D(pos, last_pos);
float travelled = horz_dist / std::cos(maneuver->pitch);
// compensate with path controller's eta factor
travelled = compensate(travelled, speed);
last_pos = pos;
durations.push_back(travelled / speed + last_dur);
return durations.back();
}
int main()
{
// Take the counts per second (frequency) at the start of the program
/*float dt = 0;
__int64 cntsPerSec = 0;
QueryPerformanceFrequency((LARGE_INTEGER*)&cntsPerSec);
float secsPerCnt = 1.0f / (float)cntsPerSec;
__int64 prevTimeStamp = 0;
QueryPerformanceCounter((LARGE_INTEGER*)&prevTimeStamp);
__int64 currTimeStamp = 0;
do
{
QueryPerformanceCounter((LARGE_INTEGER*)&currTimeStamp);
dt = (currTimeStamp - prevTimeStamp) * secsPerCnt;
std::cout << "Time: " << dt << std::endl;
}while(dt <= 10);*/
/*Clock myClock = Clock();
do
{
myClock.SetDeltaTime();
std::cout << "Time: " << myClock.GetDeltaTime() << std::endl;
}while(myClock.GetDeltaTime() <= 10);*/
Point2D point1(3.0, 0.0);
Point2D point2(1.5, 0.0);
bool bCheck= CollisionCheck2D_AABB(point1, point2) ;
std::cout << "The distance between Point1: (" << point1.x << "," << point1.y << ") and Point2: (" << point2.x << "," << point2.y << ")" << std::endl;
std::cout << "Dist: " << distance2D(point1.x, point1.y, point2.x, point2.y) << std::endl;
std::cout << "Collision check: " << bCheck << std::endl;
return 0;
}
bool
TimeProfile::parse(const IMC::YoYo* maneuver, Position& last_pos)
{
float speed = convertSpeed(maneuver);
if (speed == 0.0)
return false;
Position pos;
extractPosition(maneuver, pos);
// Use 2D distance here
float horz_dist = distance2D(pos, last_pos);
float travelled = horz_dist / std::cos(maneuver->pitch);
// compensate with path controller's eta factor
travelled = compensate(travelled, speed);
last_pos = pos;
float duration = travelled / speed;
// Update speed profile
m_speed_vec->push_back(SpeedProfile(maneuver, duration));
m_accum_dur->addDuration(duration);
return true;
}
float
Duration::distance3D(const Position& new_pos, const Position& last_pos)
{
float value = distance2D(new_pos, last_pos);
float offset = computeZOffset(new_pos, last_pos);
float slope = std::atan2(offset, value);
return value / std::cos(slope);
}
float getAngle(const Vector2f & A, const Vector2f & B)
{
float d = distance2D(A, B);
if(d > -0.001 && d < 0.001)
return 0;
if(B.y > A.y)
return acos((B.x - A.x) / d);
else
return -acos((B.x - A.x) / d);
}
void SoMinimalEnclosingCircle::screenSpaceBoundingBox() {
SbVec2f minBBox2, maxBBox2;
minBBox2 = maxBBox2 = _pointSetRaster[0];
for (int counter = 0; counter < _sizeRaster; counter++) {
if (minBBox2[0] > _pointSetRaster[counter][0]) minBBox2[0] = _pointSetRaster[counter][0];
if (minBBox2[1] > _pointSetRaster[counter][1]) minBBox2[1] = _pointSetRaster[counter][1];
if (maxBBox2[0] < _pointSetRaster[counter][0]) maxBBox2[0] = _pointSetRaster[counter][0];
if (maxBBox2[1] < _pointSetRaster[counter][1]) maxBBox2[1] = _pointSetRaster[counter][1];
}
center.setValue(minBBox2[0] + ((maxBBox2[0] - minBBox2[0]) / 2.0f), minBBox2[1] + ((maxBBox2[1] - minBBox2[1]) / 2.0f), _zValue);
radius.setValue(distance2D(minBBox2.getValue(), maxBBox2.getValue()));
}
/* Rotating */
void FreeCamera::rotate(float angle)
{
float eyeX, eyeY, eyeZ;
float centerX, centerY, centerZ;
getEyePosition(eyeX, eyeY, eyeZ);
getReferencePoint(centerX, centerY, centerZ);
float distance = distance2D(eyeX, eyeZ, centerX, centerZ);
centerX += distance*(cos(degreesToRadians(xzAngle + angle)) - cos(degreesToRadians(xzAngle)));
centerZ += distance*(sin(degreesToRadians(xzAngle + angle)) - sin(degreesToRadians(xzAngle)));
xzAngle += angle;
setReferencePoint(centerX, centerY, centerZ);
}
long shared_edges(POLYGON *PO1, POLYGON *PO2, long no_intersection, long *l_po1, long *l_po2, double *dist_centroids){
/*!
*
*
* \author Emanuele Cordano
* \date November 2008
*
* \param PO1 - (POLYGON *) the first polygon
* \param PO2 - (POLYGON *) the second polygon
* \param no_intersection - (long) integer vale which is returend in case PO1 and PO2 do not share any edge.
* \param l_po1 - (double) number of the shared edge respect to the polygon PO1
* \param l_po2 - (double) number of the shared edge respect to the polygon PO2
*
* \param dist_centroids - (double) distance between the two centroids of the polygons
*
* \return the index of the edge shared between the two polygons;
*/
long l,k,l_shared;
double distance;
int s;
l_shared=no_intersection;
s=0;
for(l=PO1->edge_indices->nl;l<=PO1->edge_indices->nh;l++){
for(k=PO2->edge_indices->nl;k<=PO2->edge_indices->nh;k++){
if (PO1->edge_indices->element[l]==PO2->edge_indices->element[k]) {
if (s==0){
l_shared=PO1->edge_indices->element[l];
*l_po1=l;
*l_po2=k;
s=1;
}else {
printf ("Warning:: polygons %ld and %ld share more than one edge (%ld,%ld)!! \n",PO1->index,PO2->index,PO1->edge_indices->element[l],l_shared);
l_shared=no_intersection;
}
}
}
}
distance=distance2D(PO1->centroid,PO2->centroid);
*dist_centroids=distance;
//printf("... PO1=%ld PO2=%ld dist=%lf",PO1->index,PO2->index,distance);
//stop_execution();
return l_shared;
}
void Level::getEntitiesInRadius(
std::list<Entity*> & entities,
const Vector2f & pos, float radius, Entity * e)
{
// TODO use the space divider with a circular intersection
// iterating on all entities
std::map<int, Entity*>::const_iterator it;
for(it = m_entities.begin(); it != m_entities.end(); it++)
{
// Except e
if(e == it->second) // adress comparison
continue;
if(distance2D(it->second->pos, pos) <= radius)
entities.push_back(it->second);
}
}
bool
Duration::parse(const IMC::YoYo* maneuver, Position& last_pos)
{
float speed = convertSpeed(maneuver);
if (speed == 0.0)
return false;
Position pos;
extractPosition(maneuver, pos);
// Use 2D distance here
float horz_dist = distance2D(pos, last_pos);
float travelled = horz_dist / std::cos(maneuver->pitch);
// compensate with path controller's eta factor
travelled = compensate(travelled, speed);
last_pos = pos;
m_accum_dur->addDuration(travelled / speed);
return true;
}
bool GlobalPlanner::FindPath(std::vector<Cell> & path) {
Cell s = Cell(currPos);
Cell t = Cell(goalPos.x(), goalPos.y(), 2);
if (occupied.find(s) != occupied.end()) {
goingBack = true;
for (int i=pathBack.size()-1; i >= 0; i -= 2){
path.push_back(pathBack[i]);
}
ROS_INFO("Current position is occupied, going back. Path size is %d", path.size());
return true;
}
if (occupied.find(t) != occupied.end()) {
ROS_INFO("Goal position is occupied");
return false;
}
ROS_INFO("Trying to find path from %d,%d to %d,%d", s.x(), s.y(), t.x(), t.y());
std::map<Cell, Cell> parent;
std::map<Cell, double> distance;
std::set<Cell> seen;
std::priority_queue< std::pair<Cell,double>,
std::vector< std::pair<Cell,double> >,
CompareDist> pq;
pq.push(std::make_pair(s, 0.0));
distance[s] = 0.0;
int numIter = 0;
while (!pq.empty() && numIter++ < maxIterations) {
auto cellDistU = pq.top(); pq.pop();
Cell u = cellDistU.first;
double d = distance[u];
if (seen.find(u) != seen.end()) {
continue;
}
seen.insert(u);
if (u == t) {
break;
}
std::vector< std::pair<Cell, double> > neighbors;
getNeighbors(u, neighbors);
for (auto cellDistV : neighbors) {
Cell v = cellDistV.first;
double risk = getRisk(v);
double newDist = d + cellDistV.second + riskFactor * risk;
double oldDist = inf;
if (distance.find(v) != distance.end()) {
oldDist = distance[v];
}
if (occupied.find(v) == occupied.end() && seen.find(v) == seen.end()
&& newDist < oldDist) {
parent[v] = u;
distance[v] = newDist;
double heuristic = newDist + overEstimateFactor*distance2D(v, t);
pq.push(std::make_pair(v, heuristic));
}
}
}
if (seen.find(t) == seen.end()) {
ROS_INFO(" Failed to find a path");
return false;
}
Cell walker = t;
pathCells.clear();
while (!(walker == s)) {
path.push_back(walker);
pathCells.insert(walker);
walker = parent[walker];
}
std::reverse(path.begin(),path.end());
ROS_INFO("Found path with %d iterations, iterations / distance squared: %f", numIter, numIter / squared(path.size()));
return true;
}
void CulledOccluderSource::cullViewEdges(ViewMap& viewMap, bool extensiveFEdgeSearch)
{
// Cull view edges by marking them as non-displayable.
// This avoids the complications of trying to delete edges from the ViewMap.
// Non-displayable view edges will be skipped over during visibility calculation.
// View edges will be culled according to their position w.r.t. the viewport proscenium (viewport + 5% border,
// or some such).
// Get proscenium boundary for culling
real viewProscenium[4];
GridHelpers::getDefaultViewProscenium(viewProscenium);
real prosceniumOrigin[2];
prosceniumOrigin[0] = (viewProscenium[1] - viewProscenium[0]) / 2.0;
prosceniumOrigin[1] = (viewProscenium[3] - viewProscenium[2]) / 2.0;
if (G.debug & G_DEBUG_FREESTYLE) {
cout << "Proscenium culling:" << endl;
cout << "Proscenium: [" << viewProscenium[0] << ", " << viewProscenium[1] << ", " << viewProscenium[2] <<
", " << viewProscenium[3] << "]"<< endl;
cout << "Origin: [" << prosceniumOrigin[0] << ", " << prosceniumOrigin[1] << "]"<< endl;
}
// A separate occluder proscenium will also be maintained, starting out the same as the viewport proscenium, and
// expanding as necessary so that it encompasses the center point of at least one feature edge in each
// retained view edge.
// The occluder proscenium will be used later to cull occluding triangles before they are inserted into the Grid.
// The occluder proscenium starts out the same size as the view proscenium
GridHelpers::getDefaultViewProscenium(occluderProscenium);
// XXX Freestyle is inconsistent in its use of ViewMap::viewedges_container and vector<ViewEdge*>::iterator.
// Probably all occurences of vector<ViewEdge*>::iterator should be replaced ViewMap::viewedges_container
// throughout the code.
// For each view edge
ViewMap::viewedges_container::iterator ve, veend;
for (ve = viewMap.ViewEdges().begin(), veend = viewMap.ViewEdges().end(); ve != veend; ve++) {
// Overview:
// Search for a visible feature edge
// If none: mark view edge as non-displayable
// Otherwise:
// Find a feature edge with center point inside occluder proscenium.
// If none exists, find the feature edge with center point closest to viewport origin.
// Expand occluder proscenium to enclose center point.
// For each feature edge, while bestOccluderTarget not found and view edge not visibile
bool bestOccluderTargetFound = false;
FEdge *bestOccluderTarget = NULL;
real bestOccluderDistance = 0.0;
FEdge *festart = (*ve)->fedgeA();
FEdge *fe = festart;
// All ViewEdges start culled
(*ve)->setIsInImage(false);
// For simple visibility calculation: mark a feature edge that is known to have a center point inside
// the occluder proscenium. Cull all other feature edges.
do {
// All FEdges start culled
fe->setIsInImage(false);
// Look for the visible edge that can most easily be included in the occluder proscenium.
if (!bestOccluderTargetFound) {
// If center point is inside occluder proscenium,
if (insideProscenium(occluderProscenium, fe->center2d())) {
// Use this feature edge for visibility deterimination
fe->setIsInImage(true);
expandGridSpaceOccluderProscenium(fe);
// Mark bestOccluderTarget as found
bestOccluderTargetFound = true;
bestOccluderTarget = fe;
}
else {
real d = distance2D(fe->center2d(), prosceniumOrigin);
// If center point is closer to viewport origin than current target
if (bestOccluderTarget == NULL || d < bestOccluderDistance) {
// Then store as bestOccluderTarget
bestOccluderDistance = d;
bestOccluderTarget = fe;
}
}
}
// If feature edge crosses the view proscenium
if (!(*ve)->isInImage() && crossesProscenium(viewProscenium, fe)) {
// Then the view edge will be included in the image
(*ve)->setIsInImage(true);
}
fe = fe->nextEdge();
} while (fe != NULL && fe != festart && !(bestOccluderTargetFound && (*ve)->isInImage()));
// Either we have run out of FEdges, or we already have the one edge we need to determine visibility
// Cull all remaining edges.
while (fe != NULL && fe != festart) {
fe->setIsInImage(false);
fe = fe->nextEdge();
}
// If bestOccluderTarget was not found inside the occluder proscenium,
// we need to expand the occluder proscenium to include it.
if ((*ve)->isInImage() && bestOccluderTarget != NULL && ! bestOccluderTargetFound) {
// Expand occluder proscenium to enclose bestOccluderTarget
Vec3r point = bestOccluderTarget->center2d();
if (point[0] < occluderProscenium[0]) {
occluderProscenium[0] = point[0];
}
else if (point[0] > occluderProscenium[1]) {
occluderProscenium[1] = point[0];
}
if (point[1] < occluderProscenium[2]) {
occluderProscenium[2] = point[1];
}
else if (point[1] > occluderProscenium[3]) {
occluderProscenium[3] = point[1];
}
// Use bestOccluderTarget for visibility determination
bestOccluderTarget->setIsInImage(true);
}
}
// We are done calculating the occluder proscenium.
// Expand the occluder proscenium by an epsilon to avoid rounding errors.
const real epsilon = 1.0e-6;
occluderProscenium[0] -= epsilon;
occluderProscenium[1] += epsilon;
occluderProscenium[2] -= epsilon;
occluderProscenium[3] += epsilon;
// For "Normal" or "Fast" style visibility computation only:
// For more detailed visibility calculation, make a second pass through the view map, marking all feature edges
// with center points inside the final occluder proscenium. All of these feature edges can be considered during
// visibility calculation.
// So far we have only found one FEdge per ViewEdge. The "Normal" and "Fast" styles of visibility computation
// want to consider many FEdges for each ViewEdge.
// Here we re-scan the view map to find any usable FEdges that we skipped on the first pass, or that have become
// usable because the occluder proscenium has been expanded since the edge was visited on the first pass.
if (extensiveFEdgeSearch) {
// For each view edge,
for (ve = viewMap.ViewEdges().begin(), veend = viewMap.ViewEdges().end(); ve != veend; ve++) {
if (!(*ve)->isInImage()) {
continue;
}
// For each feature edge,
FEdge *festart = (*ve)->fedgeA();
FEdge *fe = festart;
do {
// If not (already) visible and center point inside occluder proscenium,
if (!fe->isInImage() && insideProscenium(occluderProscenium, fe->center2d())) {
// Use the feature edge for visibility determination
fe->setIsInImage(true);
expandGridSpaceOccluderProscenium(fe);
}
fe = fe->nextEdge();
} while (fe != NULL && fe != festart);
}
}
// Up until now, all calculations have been done in camera space.
// However, the occluder source's iteration and the grid that consumes the occluders both work in gridspace,
// so we need a version of the occluder proscenium in gridspace.
// Set the gridspace occlude proscenium
}
double weight() const { return distance2D( coords ); }
/*
* Compare the minutia records contained in two finger view records.
* If the finger numbers don't match, this function just returns.
* Otherwise, the distance between the minutia (one from each view)
* is calculated. If this distance is less than the specified
* distance (given as the radius of a circle around the minutiae),
* the minutia pair is counted as overlapping.
*/
static void
compare_fvmrs(FVMR *fvmr1, FVMR *fvmr2)
{
FMD **fmds[2] = {NULL, NULL};
int mcount[2];
int mcount_common = 0;
unsigned char *paired[2] = {NULL, NULL};
int i, j, r, minj;
double **dmat = NULL;
double mind, ratio;
double meanpaireddistance = 0.0;
double meanpairedangle2 = 0.0; /* mean square diff in angle */
mcount[0] = get_fmd_count(fvmr1);
mcount[1] = get_fmd_count(fvmr2);
if ((fvmr1->finger_number != fvmr2->finger_number) ||
(mcount[0] == 0) || (mcount[1] == 0))
{
fprintf(stdout, "%d %d 0 0 0 0\n", mcount[0], mcount[1]);
return;
}
fmds[0] = (FMD **)malloc(mcount[0] * sizeof(FMD *));
if (fmds[0] == NULL)
ALLOC_ERR_OUT("FMR Array");
if (get_fmds(fvmr1, fmds[0]) != mcount[0])
ERR_OUT("getting FMRs from first FVMR");
fmds[1] = (FMD **)malloc(mcount[1] * sizeof(FMD *));
if (fmds[1] == NULL)
ALLOC_ERR_OUT("FMR Array");
if (get_fmds(fvmr2, fmds[1]) != mcount[1])
ERR_OUT("getting FMRs from second FVMR");
if (v_opt > 1) {
printf("The first FVMR:\n");
(void)print_fvmr(stdout, fvmr1);
printf("The second FVMR:\n");
(void)print_fvmr(stdout, fvmr2);
}
paired[0] = (unsigned char *)malloc(mcount[0] * sizeof(unsigned char));
if (paired[0] == NULL)
ALLOC_ERR_OUT("Paired flags");
paired[1] = (unsigned char *)malloc(mcount[1] * sizeof(unsigned char));
if (paired[1] == NULL)
ALLOC_ERR_OUT("Paired flags");
bzero((void *)paired[0], mcount[0]);
bzero((void *)paired[1], mcount[1]);
dmat = (double **)malloc(mcount[0] * sizeof(double *));
if (dmat == NULL)
ALLOC_ERR_OUT("Distance array");
for (i = 0; i < mcount[0]; i++) {
dmat[i] = (double *)malloc(mcount[1] * sizeof(double));
if (dmat[i] == NULL)
ALLOC_ERR_OUT("Distance array");
}
for (i = 0; i < mcount[0]; i++)
for (j = 0; j < mcount[1]; j++)
dmat[i][j] = distance2D(fmds[0][i], fmds[1][j]);
for (r = 0; r <= r_opt; r++) {
for (i = 0; i < mcount[0]; i++) {
if (paired[0][i] == 1)
continue;
mind = 100000;
minj = -1;
for (j = 0; j < mcount[1]; j++) {
if ((paired[1][j] == 0) &&
(dmat[i][j] < mind)) {
mind = dmat[i][j];
minj = j;
}
}
/* There will be times when everything in the second record
* is paired whence minj will still be -1. This might occur
* when there are fewer minutiae in the second template than
* in the first. But if we have found a close candidate, then
* determine if it is close enough and not already paired.
*/
if ((minj > -1) && (mind <= r) &&
(paired[1][minj] == 0)) {
if (v_opt > 0) {
printf("Distance of %.2f calculated "
"for this matching pair:\n",
dmat[i][minj]);
print_fmd(stdout, fmds[0][i]);
print_fmd(stdout, fmds[1][minj]);
printf("-----------------------\n");
}
paired[0][i] = paired[1][minj] = 1;
meanpaireddistance += dmat[i][minj];
meanpairedangle2 += dtheta2(fmds[0][i], fmds[1][minj]);
mcount_common++;
}
}
}
/* Final summary stats are:
* 1. the size of the intersection set divided by the size of the smaller of the
* number of minutiae in the two input templates - this will be on range [0,1].
* 2. the mean displacement of those minutiae that are found to be paired
*/
ratio = (double)mcount_common / (double)((mcount[0] < mcount[1]) ?
mcount[0] : mcount[1]);
if (mcount_common > 0)
{
meanpaireddistance /= (double)mcount_common;
meanpairedangle2 = sqrt(meanpairedangle2 / (double)mcount_common); /* rms */
}
fprintf(stdout, "%d %d %d %lf %lf %lf\n", mcount[0], mcount[1], mcount_common,
ratio, meanpaireddistance, meanpairedangle2);
total_overlapping_count += mcount_common;
err_out:
if (dmat != NULL) {
for (i = 0; i < mcount[0]; i++)
if (dmat[i] != NULL)
free(dmat[i]);
free (dmat);
}
if (paired[0] != NULL)
free (paired[0]);
if (paired[1] != NULL)
free (paired[1]);
if (fmds[0] != NULL)
free(fmds[0]);
if (fmds[1] != NULL)
free(fmds[1]);
return;
}