@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;
}
}
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;
}
}
