@Override
public Vector2D getDimensions() {
double width = 0, height = 0;
for (Text text : this.text) {
Vector2D dim = text.getDimensions();
height = Math.max(height, dim.getY());
width += dim.getX();
}
return new Vector2D(width, height);
}
@Override
public void handleMessage(Message msg) {
switch (msg.what) {
case LocalisationService.LOC_DISPLAY_POINT:
if (msg.obj != null) {
Vector2D pos = (Vector2D) msg.obj;
m_locServicePoint.setPos((float) pos.getX(), (float) pos.getY());
m_map.invalidate();
}
break;
case LocalisationService.LOC_DISPLAY_LINE:
Log.d(LOG_TAG, "LocalisationService - Display : line");
break;
case LocalisationService.LOC_DISPLAY_TEXT:
Log.d(LOG_TAG, "LocalisationService - Display : " + (String) msg.obj);
break;
default:
Log.d(LOG_TAG, "LocalisationService - Display : Erreur message");
break;
}
}
/**
* Checks if the given point is located within the convex quadrilateral.
*
* @param point the point to check
* @param quadrilateralPoints the convex quadrilateral, represented by 4 points
* @return {@code true} if the point is inside the quadrilateral, {@code false} otherwise
*/
private static boolean insideQuadrilateral(
final Vector2D point, final List<Vector2D> quadrilateralPoints) {
Vector2D p1 = quadrilateralPoints.get(0);
Vector2D p2 = quadrilateralPoints.get(1);
if (point.equals(p1) || point.equals(p2)) {
return true;
}
// get the location of the point relative to the first two vertices
final double last = point.crossProduct(p1, p2);
final int size = quadrilateralPoints.size();
// loop through the rest of the vertices
for (int i = 1; i < size; i++) {
p1 = p2;
p2 = quadrilateralPoints.get((i + 1) == size ? 0 : i + 1);
if (point.equals(p1) || point.equals(p2)) {
return true;
}
// do side of line test: multiply the last location with this location
// if they are the same sign then the operation will yield a positive result
// -x * -y = +xy, x * y = +xy, -x * y = -xy, x * -y = -xy
if (last * point.crossProduct(p1, p2) < 0) {
return false;
}
}
return true;
}
@Override
public void handleMessage(Message msg) {
switch (msg.what) {
case LocalisationService.LOC_POSITION_UPDATED:
algoClosestPointOnPath((Vector2D) msg.obj);
m_path.setCurrentPosition(
(float) m_currentPosition.getX(),
(float) m_currentPosition.getY(),
m_currentStep);
m_map.invalidate();
Log.d(LOG_TAG, "LocalisationService : nouvelle position");
break;
case LocalisationService.LOC_ALGO_START:
Log.d(LOG_TAG, "LocalisationService : calculs de localisation en cours...");
break;
case LocalisationService.LOC_ALGO_STOP:
Log.d(LOG_TAG, "LocalisationService : arr�t des calculs de localisation");
break;
case LocalisationService.LOC_ALGO_ERROR:
Log.d(LOG_TAG, "LocalisationService : " + (String) msg.obj);
break;
}
}
/**
* Returns a point set that is reduced by all points for which it is safe to assume that they are
* not part of the convex hull.
*
* @param points the original point set
* @return a reduced point set, useful as input for convex hull algorithms
*/
public static Collection<Vector2D> reducePoints(final Collection<Vector2D> points) {
// find the leftmost point
int size = 0;
Vector2D minX = null;
Vector2D maxX = null;
Vector2D minY = null;
Vector2D maxY = null;
for (Vector2D p : points) {
if (minX == null || p.getX() < minX.getX()) {
minX = p;
}
if (maxX == null || p.getX() > maxX.getX()) {
maxX = p;
}
if (minY == null || p.getY() < minY.getY()) {
minY = p;
}
if (maxY == null || p.getY() > maxY.getY()) {
maxY = p;
}
size++;
}
if (size < 4) {
return points;
}
final List<Vector2D> quadrilateral = buildQuadrilateral(minY, maxX, maxY, minX);
// if the quadrilateral is not well formed, e.g. only 2 points, do not attempt to reduce
if (quadrilateral.size() < 3) {
return points;
}
final List<Vector2D> reducedPoints = new ArrayList<Vector2D>(quadrilateral);
for (final Vector2D p : points) {
// check all points if they are within the quadrilateral
// in which case they can not be part of the convex hull
if (!insideQuadrilateral(p, quadrilateral)) {
reducedPoints.add(p);
}
}
return reducedPoints;
}
private void algoClosestPointOnPath(Vector2D pos2D) {
Step selectedStep = null;
CheckPoint selectedCheckPoint = null;
double distanceMaxStep = Double.MAX_VALUE;
double distanceMaxCheckPoint = Double.MAX_VALUE;
ArrayList<Step> steps = m_path.getSteps();
for (Step step : steps) {
Vector2D posA = step.getCheckPointA().getPos(); // point A du segment
Vector2D posB = step.getCheckPointB().getPos(); // point B du segment
Line stepLine = new Line(posA, posB); // ligne correspondant au segment
Segment stepSeg = new Segment(posA, posB, stepLine); // segment
double distancePosSegment = stepSeg.distance(pos2D); // distance segment <-> position initiale
// Test de l'existence d'un projeté orthogonal :
// si la distance segment <-> position initiale est inférieure aux distances point A <->
// position initiale et point B <-> position initiale
if ((distancePosSegment < pos2D.distance(posA))
&& (distancePosSegment < pos2D.distance(posB))) {
if (distancePosSegment
< distanceMaxStep) { // si le segment est plus proche de la position initiale que le
// précédemment sélectionné
selectedStep = step;
distanceMaxStep = distancePosSegment;
}
} else { // sinon on compare le point le plus proche de la position initiale au meilleur
// precedemment selectionné
if (pos2D.distance(posA) < pos2D.distance(posB)) {
if (pos2D.distance(posA) < distanceMaxCheckPoint) {
selectedCheckPoint = step.getCheckPointA();
distanceMaxCheckPoint = pos2D.distance(posA);
}
} else {
if (pos2D.distance(posB) < distanceMaxCheckPoint) {
selectedCheckPoint = step.getCheckPointB();
distanceMaxCheckPoint = pos2D.distance(posB);
}
}
}
}
if (selectedStep != null) {
if (selectedCheckPoint
!= null) { // si un step et un checkpoint sont s�lectionn�s, on choisi le plus proche de
// la position initiale
if (distanceMaxCheckPoint < distanceMaxStep) {
m_currentPosition = selectedCheckPoint.getPos();
m_currentStep = null;
return;
}
}
Vector2D posA = selectedStep.getCheckPointA().getPos(); // point A du segment
Vector2D posB = selectedStep.getCheckPointB().getPos(); // point B du segment
double distanceSegment = Vector2D.distanceSq(posA, posB);
if (distanceSegment == 0.0)
m_currentPosition = pos2D; // la position initiale est sur le step de longueur nulle
else {
Vector2D p1 = pos2D.subtract(posA);
Vector2D p2 = posB.subtract(posA);
double t = p1.dotProduct(p2) / distanceSegment;
m_currentPosition =
posB.subtract(posA).scalarMultiply(t).add(posA); // r�cup�ration du projet� orthogonal
}
m_currentStep = selectedStep;
} else if (selectedCheckPoint != null) {
m_currentPosition = selectedCheckPoint.getPos();
m_currentStep = null;
}
}
