diff --git a/.gitattributes b/.gitattributes deleted file mode 100644 index dfe07704..00000000 --- a/.gitattributes +++ /dev/null @@ -1,2 +0,0 @@ -# Auto detect text files and perform LF normalization -* text=auto diff --git a/.gitignore b/.gitignore deleted file mode 100644 index d4c3a57e..00000000 --- a/.gitignore +++ /dev/null @@ -1,16 +0,0 @@ -*.iml -.gradle -/local.properties -/.idea/caches -/.idea/libraries -/.idea/modules.xml -/.idea/workspace.xml -/.idea/navEditor.xml -/.idea/assetWizardSettings.xml -.DS_Store -/build -/captures -.externalNativeBuild -.cxx -local.properties -/.idea/ diff --git a/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java b/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java index cadb6037..cac78775 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java +++ b/app/src/main/java/com/openpositioning/PositionMe/data/remote/FloorplanApiClient.java @@ -14,6 +14,7 @@ import java.io.IOException; import java.util.ArrayList; import java.util.Collections; +import java.util.Comparator; import java.util.Iterator; import java.util.List; @@ -372,13 +373,56 @@ private List parseMapShapes(String mapShapesJson) { while (it.hasNext()) { keys.add(it.next()); } - Collections.sort(keys); + // Sort floor keys in building-logical order: + // Basement levels (B*) first, then Ground (G*), then upper floors (F1, F2, ...) + Collections.sort(keys, new Comparator() { + @Override + public int compare(String a, String b) { + return floorKeyOrder(a) - floorKeyOrder(b); + } + + private int floorKeyOrder(String key) { + String upper = key.toUpperCase(); + // Basement levels: B1=-2, B2=-3, etc. + if (upper.startsWith("B")) { + try { + return -Integer.parseInt(upper.substring(1)) - 1; + } catch (NumberFormatException e) { + return -1; + } + } + // Ground floor + if (upper.startsWith("G")) return 0; + // Upper floors: F1=1, F2=2, etc. + if (upper.startsWith("F")) { + try { + return Integer.parseInt(upper.substring(1)); + } catch (NumberFormatException e) { + return 100; + } + } + // Pure numeric keys + try { + return Integer.parseInt(upper); + } catch (NumberFormatException e) { + return 100; + } + } + }); - for (String key : keys) { + Log.e(TAG, "FLOOR_PARSE: building floor keys (sorted): " + keys); + + for (int idx = 0; idx < keys.size(); idx++) { + String key = keys.get(idx); JSONObject floorCollection = root.getJSONObject(key); String displayName = floorCollection.optString("name", key); JSONArray features = floorCollection.optJSONArray("features"); + Log.e(TAG, "FLOOR_PARSE: index=" + idx + + " | key=\"" + key + "\"" + + " | displayName=\"" + displayName + "\"" + + " | features=" + (features != null ? features.length() : 0)); + List shapeFeatures = new ArrayList<>(); if (features != null) { for (int i = 0; i < features.length(); i++) { diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java index 340843ca..6612f988 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java @@ -35,6 +35,7 @@ import java.util.ArrayList; import java.util.List; +import android.util.Log; import android.widget.Toast; @@ -65,6 +66,8 @@ public class RecordingFragment extends Fragment { + private static final String TAG = "RECORDING_DEBUG"; + // UI elements private MaterialButton completeButton, cancelButton; private ImageView recIcon; @@ -147,7 +150,7 @@ public void onViewCreated(@NonNull View view, recIcon = view.findViewById(R.id.redDot); timeRemaining = view.findViewById(R.id.timeRemainingBar); view.findViewById(R.id.btn_test_point).setOnClickListener(v -> onAddTestPoint()); - + view.findViewById(R.id.btn_refresh_position).setOnClickListener(v -> onRefreshPosition()); // Hide or initialize default values gnssError.setVisibility(View.GONE); @@ -247,51 +250,161 @@ private void onAddTestPoint() { trajectoryMapFragment.addTestPointMarker(idx, ts, cur); } + /** + * Manually refresh the fused position by resetting the particle filter + * and re-initializing it with the current GNSS or WiFi position. + * This is useful when the position has drifted significantly and needs correction. + */ + private void onRefreshPosition() { + // Record old fused position for logging + LatLng oldFused = sensorFusion.getFusedPosition(); + + // Try GNSS position first (current, not start) + float[] gnssLatLng = sensorFusion.getGNSSLatitude(false); + double newLat = 0; + double newLng = 0; + String source = null; + + if (gnssLatLng != null && (gnssLatLng[0] != 0 || gnssLatLng[1] != 0)) { + newLat = gnssLatLng[0]; + newLng = gnssLatLng[1]; + source = "GNSS"; + } else { + // Fallback to WiFi positioning + LatLng wifiPos = sensorFusion.getLatLngWifiPositioning(); + if (wifiPos != null) { + newLat = wifiPos.latitude; + newLng = wifiPos.longitude; + source = "WiFi"; + } + } + + if (source != null) { + // Reset and re-initialize the particle filter at the new position + sensorFusion.getPositionFusion().reset(); + sensorFusion.getPositionFusion().init(newLat, newLng); + + Log.e("POSITION_REFRESH", "Position refreshed using " + source + + " | old=(" + (oldFused != null ? String.format("%.7f", oldFused.latitude) + + "," + String.format("%.7f", oldFused.longitude) : "null") + + ") | new=(" + String.format("%.7f", newLat) + + "," + String.format("%.7f", newLng) + ")"); + + Toast.makeText(requireContext(), + getString(R.string.refresh_position_success), + Toast.LENGTH_SHORT).show(); + } else { + Log.e("POSITION_REFRESH", "No valid GNSS or WiFi position available for refresh"); + Toast.makeText(requireContext(), + getString(R.string.refresh_position_no_source), + Toast.LENGTH_SHORT).show(); + } + } /** * Update the UI with sensor data and pass map updates to TrajectoryMapFragment. */ private void updateUIandPosition() { float[] pdrValues = sensorFusion.getSensorValueMap().get(SensorTypes.PDR); - if (pdrValues == null) return; + if (pdrValues == null) { + Log.e(TAG, "PDR values are null, skipping update"); + return; + } // Distance - distance += Math.sqrt(Math.pow(pdrValues[0] - previousPosX, 2) - + Math.pow(pdrValues[1] - previousPosY, 2)); + float dx = pdrValues[0] - previousPosX; + float dy = pdrValues[1] - previousPosY; + double stepDist = Math.sqrt(dx * dx + dy * dy); + distance += stepDist; distanceTravelled.setText(getString(R.string.meter, String.format("%.2f", distance))); // Elevation float elevationVal = sensorFusion.getElevation(); elevation.setText(getString(R.string.elevation, String.format("%.1f", elevationVal))); - // Current location - // Convert PDR coordinates to actual LatLng if you have a known starting lat/lon - // Or simply pass relative data for the TrajectoryMapFragment to handle - // For example: + // Log PDR state periodically (every ~1s = every 5th call at 200ms interval) + if (((int)(distance * 10)) % 5 == 0) { + Log.e(TAG, "=== PDR State ==="); + Log.e(TAG, "PDR raw: x=" + pdrValues[0] + " y=" + pdrValues[1]); + Log.e(TAG, "PDR delta: dx=" + String.format("%.4f", dx) + + " dy=" + String.format("%.4f", dy) + + " stepDist=" + String.format("%.3f", stepDist) + "m"); + Log.e(TAG, "Total distance: " + String.format("%.2f", distance) + + "m | Elevation: " + String.format("%.1f", elevationVal) + + "m | Elevator: " + sensorFusion.getElevator()); + } + + // Current location — use fused (smooth) or raw PDR based on toggle + LatLng fusedPos = sensorFusion.getFusedPosition(); float[] latLngArray = sensorFusion.getGNSSLatitude(true); - if (latLngArray != null) { - LatLng oldLocation = trajectoryMapFragment.getCurrentLocation(); // or store locally + boolean useSmooth = trajectoryMapFragment != null && trajectoryMapFragment.isSmoothEnabled(); + + if (useSmooth && fusedPos != null) { + // Smooth ON: use fused position (PDR + GNSS + WiFi corrections) + LatLng oldLocation = trajectoryMapFragment.getCurrentLocation(); + if (oldLocation == null) { + Log.e(TAG, "=== Initial Fused Position ==="); + Log.e(TAG, "Fused pos: " + fusedPos.latitude + ", " + fusedPos.longitude); + } + + if (trajectoryMapFragment != null) { + float orientDeg = (float) Math.toDegrees(sensorFusion.passOrientation()); + trajectoryMapFragment.updateUserLocation(fusedPos, orientDeg); + + // Log fused vs raw PDR comparison periodically + if (latLngArray != null && ((int)(distance * 10)) % 10 == 0) { + LatLng pdrOnly = UtilFunctions.calculateNewPos( + new LatLng(latLngArray[0], latLngArray[1]), + pdrValues); + double fusedPdrDiff = UtilFunctions.distanceBetweenPoints(fusedPos, pdrOnly); + Log.e(TAG, "=== Fusion vs Raw PDR ==="); + Log.e(TAG, "Fused: " + String.format("%.7f", fusedPos.latitude) + + ", " + String.format("%.7f", fusedPos.longitude)); + Log.e(TAG, "RawPDR: " + String.format("%.7f", pdrOnly.latitude) + + ", " + String.format("%.7f", pdrOnly.longitude)); + Log.e(TAG, "Fused-PDR diff: " + String.format("%.2f", fusedPdrDiff) + "m"); + } + } + } else if (latLngArray != null) { + // Smooth OFF or fusion not initialized: use raw PDR + LatLng oldLocation = trajectoryMapFragment.getCurrentLocation(); LatLng newLocation = UtilFunctions.calculateNewPos( oldLocation == null ? new LatLng(latLngArray[0], latLngArray[1]) : oldLocation, - new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } + new float[]{ dx, dy } ); - // Pass the location + orientation to the map + if (oldLocation == null) { + Log.e(TAG, "=== Initial Position (PDR" + (useSmooth ? " fallback" : " raw") + ") ==="); + Log.e(TAG, "Start location: lat=" + latLngArray[0] + " lng=" + latLngArray[1]); + } else if (stepDist > 2.0) { + Log.e(TAG, "WARNING: Large single step: " + String.format("%.2f", stepDist) + + "m | dx=" + dx + " dy=" + dy); + } + if (trajectoryMapFragment != null) { - trajectoryMapFragment.updateUserLocation(newLocation, - (float) Math.toDegrees(sensorFusion.passOrientation())); + float orientDeg = (float) Math.toDegrees(sensorFusion.passOrientation()); + trajectoryMapFragment.updateUserLocation(newLocation, orientDeg); } + } else { + Log.e(TAG, "WARNING: No position available (fusion=null, startGNSS=null)"); } - // GNSS logic if you want to show GNSS error, etc. + // GNSS logic float[] gnss = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); if (gnss != null && trajectoryMapFragment != null) { - // If user toggles showing GNSS in the map, call e.g. if (trajectoryMapFragment.isGnssEnabled()) { LatLng gnssLocation = new LatLng(gnss[0], gnss[1]); LatLng currentLoc = trajectoryMapFragment.getCurrentLocation(); + + Log.e(TAG, "=== GNSS Display ==="); + Log.e(TAG, "GNSS location: " + gnss[0] + ", " + gnss[1]); if (currentLoc != null) { double errorDist = UtilFunctions.distanceBetweenPoints(currentLoc, gnssLocation); + Log.e(TAG, "PDR location: " + currentLoc.latitude + ", " + currentLoc.longitude); + Log.e(TAG, "GNSS-PDR error: " + String.format("%.2f", errorDist) + "m"); + if (errorDist > 100) { + Log.e(TAG, "WARNING: GNSS-PDR divergence >100m! Possible GNSS or PDR issue"); + } gnssError.setVisibility(View.VISIBLE); gnssError.setText(String.format(getString(R.string.gnss_error) + "%.2fm", errorDist)); } @@ -302,6 +415,27 @@ private void updateUIandPosition() { } } + // WiFi positioning — display on map + log + LatLng wifiPos = sensorFusion.getLatLngWifiPositioning(); + if (wifiPos != null && trajectoryMapFragment != null) { + if (trajectoryMapFragment.isWifiEnabled()) { + trajectoryMapFragment.updateWiFi(wifiPos); + } else { + trajectoryMapFragment.clearWiFi(); + } + + if (((int)(distance * 10)) % 10 == 0) { + Log.e(TAG, "=== WiFi Position Status ==="); + Log.e(TAG, "WiFi pos: " + wifiPos.latitude + ", " + wifiPos.longitude + + " floor=" + sensorFusion.getWifiFloor()); + LatLng currentLoc = trajectoryMapFragment.getCurrentLocation(); + if (currentLoc != null) { + double wifiPdrDist = UtilFunctions.distanceBetweenPoints(currentLoc, wifiPos); + Log.e(TAG, "WiFi-PDR distance: " + String.format("%.2f", wifiPdrDist) + "m"); + } + } + } + // Update previous previousPosX = pdrValues[0]; previousPosY = pdrValues[1]; diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java index 0951e85a..7fc5b9b0 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java @@ -300,12 +300,18 @@ private void onBuildingSelected(String buildingName, Polygon polygon) { // Compute building centre from polygon points LatLng center = computePolygonCenter(polygon); - // Move the marker to building centre + // Use GNSS position if it falls inside the building, otherwise use building centre + float[] gnss = sensorFusion.getGNSSLatitude(false); + LatLng gnssPos = new LatLng(gnss[0], gnss[1]); + boolean gnssValid = gnss[0] != 0 || gnss[1] != 0; + LatLng bestStart = (gnssValid && isPointInPolygon(gnssPos, polygon)) ? gnssPos : center; + + // Move the marker to the best start position if (startMarker != null) { - startMarker.setPosition(center); + startMarker.setPosition(bestStart); } - startPosition[0] = (float) center.latitude; - startPosition[1] = (float) center.longitude; + startPosition[0] = (float) bestStart.latitude; + startPosition[1] = (float) bestStart.longitude; // Zoom to the building mMap.animateCamera(CameraUpdateFactory.newLatLngZoom(center, 20f)); @@ -426,6 +432,30 @@ private String formatBuildingName(String apiName) { return sb.toString(); } + /** + * Ray-casting algorithm to test if a point lies inside a polygon. + * + * @param point the point to test + * @param polygon the polygon to test against + * @return true if the point is inside the polygon + */ + private boolean isPointInPolygon(LatLng point, Polygon polygon) { + List vertices = polygon.getPoints(); + int n = vertices.size(); + boolean inside = false; + for (int i = 0, j = n - 1; i < n; j = i++) { + LatLng vi = vertices.get(i); + LatLng vj = vertices.get(j); + if ((vi.latitude > point.latitude) != (vj.latitude > point.latitude) + && point.longitude < (vj.longitude - vi.longitude) + * (point.latitude - vi.latitude) / (vj.latitude - vi.latitude) + + vi.longitude) { + inside = !inside; + } + } + return inside; + } + /** * Computes the centroid of a Google Maps Polygon by averaging all vertices. * @@ -468,9 +498,9 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat } if (requireActivity() instanceof RecordingActivity) { - // Start sensor recording + set the start location - sensorFusion.startRecording(); + // Set start location BEFORE recording so PositionFusion.init() sees it sensorFusion.setStartGNSSLatitude(startPosition); + sensorFusion.startRecording(); // Write trajectory_id, initial_position and initial heading to protobuf sensorFusion.writeInitialMetadata(); diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index 479ea51b..6ef29eb6 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java @@ -29,9 +29,13 @@ import com.google.android.gms.maps.GoogleMap; import com.google.android.gms.maps.SupportMapFragment; import com.google.android.gms.maps.model.*; +import com.google.android.gms.maps.model.Circle; +import com.google.android.gms.maps.model.CircleOptions; import java.util.ArrayList; +import java.util.HashMap; import java.util.List; +import java.util.Map; /** @@ -61,15 +65,29 @@ public class TrajectoryMapFragment extends Fragment { private LatLng currentLocation; // Stores the user's current location private Marker orientationMarker; // Marker representing user's heading private Marker gnssMarker; // GNSS position marker + private Marker wifiMarker; // WiFi position marker // Keep test point markers so they can be cleared when recording ends private final List testPointMarkers = new ArrayList<>(); - private Polyline polyline; // Polyline representing user's movement path + // Per-floor polyline storage: only the current floor's segments are visible + private final Map> floorPolylines = new HashMap<>(); + private Polyline activePolyline; // Current segment on the active floor + private int polylineFloor = -1; // Floor index of the active polyline segment + private boolean isRed = true; // Tracks whether the polyline color is red private boolean isGnssOn = false; // Tracks if GNSS tracking is enabled + private boolean isWifiOn = false; // Tracks if WiFi tracking is enabled + private boolean isSmoothOn = true; // Tracks if smooth filter (fusion) is enabled private Polyline gnssPolyline; // Polyline for GNSS path + private Polyline wifiPolyline; // Polyline for WiFi path private LatLng lastGnssLocation = null; // Stores the last GNSS location + private LatLng lastWifiLocation = null; // Stores the last WiFi location + + // Color-coded observation dot markers (last N from each source) + private static final int MAX_OBSERVATION_DOTS = 20; + private final List gnssObsDots = new ArrayList<>(); + private final List wifiObsDots = new ArrayList<>(); private LatLng pendingCameraPosition = null; // Stores pending camera movement private boolean hasPendingCameraMove = false; // Tracks if camera needs to move @@ -90,7 +108,9 @@ public class TrajectoryMapFragment extends Fragment { private Spinner switchMapSpinner; private SwitchMaterial gnssSwitch; + private SwitchMaterial wifiSwitch; private SwitchMaterial autoFloorSwitch; + private SwitchMaterial smoothSwitch; private com.google.android.material.floatingactionbutton.FloatingActionButton floorUpButton, floorDownButton; private TextView floorLabel; @@ -119,7 +139,9 @@ public void onViewCreated(@NonNull View view, // Grab references to UI controls switchMapSpinner = view.findViewById(R.id.mapSwitchSpinner); gnssSwitch = view.findViewById(R.id.gnssSwitch); + wifiSwitch = view.findViewById(R.id.wifiSwitch); autoFloorSwitch = view.findViewById(R.id.autoFloor); + smoothSwitch = view.findViewById(R.id.smoothSwitch); floorUpButton = view.findViewById(R.id.floorUpButton); floorDownButton = view.findViewById(R.id.floorDownButton); floorLabel = view.findViewById(R.id.floorLabel); @@ -168,18 +190,36 @@ public void onMapReady(@NonNull GoogleMap googleMap) { } }); - // Color switch + // WiFi Switch + wifiSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { + isWifiOn = isChecked; + if (!isChecked && wifiMarker != null) { + wifiMarker.remove(); + wifiMarker = null; + } + }); + + // Smooth Filter Switch + smoothSwitch.setChecked(true); + smoothSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { + isSmoothOn = isChecked; + Log.e(TAG, "Smooth filter " + (isChecked ? "ON (fused)" : "OFF (raw PDR)")); + }); + + // Color switch — applies to all floor polyline segments switchColorButton.setOnClickListener(v -> { - if (polyline != null) { - if (isRed) { - switchColorButton.setBackgroundColor(Color.BLACK); - polyline.setColor(Color.BLACK); - isRed = false; - } else { - switchColorButton.setBackgroundColor(Color.RED); - polyline.setColor(Color.RED); - isRed = true; - } + int newColor; + if (isRed) { + newColor = Color.BLACK; + switchColorButton.setBackgroundColor(Color.BLACK); + isRed = false; + } else { + newColor = Color.RED; + switchColorButton.setBackgroundColor(Color.RED); + isRed = true; + } + for (List segs : floorPolylines.values()) { + for (Polyline p : segs) p.setColor(newColor); } }); @@ -198,6 +238,7 @@ public void onMapReady(@NonNull GoogleMap googleMap) { autoFloorSwitch.setChecked(false); if (indoorMapManager != null) { indoorMapManager.increaseFloor(); + onFloorChanged(indoorMapManager.getCurrentFloor()); updateFloorLabel(); } }); @@ -206,6 +247,7 @@ public void onMapReady(@NonNull GoogleMap googleMap) { autoFloorSwitch.setChecked(false); if (indoorMapManager != null) { indoorMapManager.decreaseFloor(); + onFloorChanged(indoorMapManager.getCurrentFloor()); updateFloorLabel(); } }); @@ -233,12 +275,9 @@ private void initMapSettings(GoogleMap map) { // Initialize indoor manager indoorMapManager = new IndoorMapManager(map); - // Initialize an empty polyline - polyline = map.addPolyline(new PolylineOptions() - .color(Color.RED) - .width(5f) - .add() // start empty - ); + // Per-floor polyline: will be created on first position update + activePolyline = null; + polylineFloor = -1; // GNSS path in blue gnssPolyline = map.addPolyline(new PolylineOptions() @@ -246,6 +285,13 @@ private void initMapSettings(GoogleMap map) { .width(5f) .add() // start empty ); + + // WiFi path in green + wifiPolyline = map.addPolyline(new PolylineOptions() + .color(Color.GREEN) + .width(5f) + .add() // start empty + ); } @@ -310,12 +356,23 @@ public void onNothingSelected(AdapterView parent) {} public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { if (gMap == null) return; + // Clamp position to building boundary (outer wall detection) + if (indoorMapManager != null) { + newLocation = indoorMapManager.clampToBuildingBoundary(newLocation); + } + + // Constrain to inner walls: slide along side walls, bounce off front walls + if (indoorMapManager != null && this.currentLocation != null) { + newLocation = indoorMapManager.constrainToWalls(this.currentLocation, newLocation); + } + // Keep track of current location LatLng oldLocation = this.currentLocation; this.currentLocation = newLocation; // If no marker, create it if (orientationMarker == null) { + Log.e(TAG, "PDR marker created at: " + newLocation.latitude + ", " + newLocation.longitude); orientationMarker = gMap.addMarker(new MarkerOptions() .position(newLocation) .flat(true) @@ -333,32 +390,46 @@ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { gMap.moveCamera(CameraUpdateFactory.newLatLng(newLocation)); } - // Extend polyline if movement occurred - /*if (oldLocation != null && !oldLocation.equals(newLocation) && polyline != null) { - List points = new ArrayList<>(polyline.getPoints()); - points.add(newLocation); - polyline.setPoints(points); - }*/ - // Extend polyline - if (polyline != null) { - List points = new ArrayList<>(polyline.getPoints()); + // Detect large jumps on the red PDR polyline + if (oldLocation != null) { + double pdrJump = UtilFunctions.distanceBetweenPoints(oldLocation, newLocation); + if (pdrJump > 10) { + Log.e(TAG, "WARNING: PDR polyline large jump " + String.format("%.1f", pdrJump) + + "m from (" + oldLocation.latitude + "," + oldLocation.longitude + + ") to (" + newLocation.latitude + "," + newLocation.longitude + ")"); + } + } + + // Update indoor map overlay (before polyline so we know the current floor) + if (indoorMapManager != null) { + indoorMapManager.setCurrentLocation(newLocation); + setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE); + } + + // Per-floor polyline: start a new segment when floor changes + int currentFloorIdx = (indoorMapManager != null) ? indoorMapManager.getCurrentFloor() : 0; + if (activePolyline == null || currentFloorIdx != polylineFloor) { + // Floor changed or first segment — create a new polyline for this floor + startNewPolylineSegment(currentFloorIdx, newLocation); + } + + // Extend the active polyline segment + if (activePolyline != null) { + List points = new ArrayList<>(activePolyline.getPoints()); - // First position fix: add the first polyline point if (oldLocation == null) { points.add(newLocation); - polyline.setPoints(points); + activePolyline.setPoints(points); } else if (!oldLocation.equals(newLocation)) { - // Subsequent movement: append a new polyline point points.add(newLocation); - polyline.setPoints(points); + activePolyline.setPoints(points); } - } - - // Update indoor map overlay - if (indoorMapManager != null) { - indoorMapManager.setCurrentLocation(newLocation); - setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE); + if (points.size() % 20 == 0) { + Log.e(TAG, "PDR polyline total points: " + points.size() + + " | floor=" + currentFloorIdx + + " | current pos: " + newLocation.latitude + ", " + newLocation.longitude); + } } } @@ -422,6 +493,7 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { if (gnssMarker == null) { // Create the GNSS marker for the first time + Log.e(TAG, "GNSS marker created at: " + gnssLocation.latitude + ", " + gnssLocation.longitude); gnssMarker = gMap.addMarker(new MarkerOptions() .position(gnssLocation) .title("GNSS Position") @@ -434,12 +506,23 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { // Add a segment to the blue GNSS line, if this is a new location if (lastGnssLocation != null && !lastGnssLocation.equals(gnssLocation)) { + double jumpDist = UtilFunctions.distanceBetweenPoints(lastGnssLocation, gnssLocation); + if (jumpDist > 50) { + Log.e(TAG, "WARNING: GNSS polyline jump " + String.format("%.1f", jumpDist) + + "m from (" + lastGnssLocation.latitude + "," + lastGnssLocation.longitude + + ") to (" + gnssLocation.latitude + "," + gnssLocation.longitude + ")"); + } List gnssPoints = new ArrayList<>(gnssPolyline.getPoints()); gnssPoints.add(gnssLocation); gnssPolyline.setPoints(gnssPoints); + Log.e(TAG, "GNSS polyline points: " + gnssPoints.size() + + " | latest: " + gnssLocation.latitude + ", " + gnssLocation.longitude); } lastGnssLocation = gnssLocation; } + + // Add color-coded observation dot + addGnssObservationDot(gnssLocation); } @@ -460,6 +543,93 @@ public boolean isGnssEnabled() { return isGnssOn; } + /** + * Update WiFi position marker and polyline on the map. + * + * @param wifiLocation the WiFi-estimated position + */ + public void updateWiFi(@NonNull LatLng wifiLocation) { + if (gMap == null) return; + if (!isWifiOn) return; + + if (wifiMarker == null) { + Log.e(TAG, "WiFi marker created at: " + wifiLocation.latitude + ", " + wifiLocation.longitude); + wifiMarker = gMap.addMarker(new MarkerOptions() + .position(wifiLocation) + .title("WiFi Position") + .icon(BitmapDescriptorFactory + .defaultMarker(BitmapDescriptorFactory.HUE_GREEN))); + lastWifiLocation = wifiLocation; + } else { + wifiMarker.setPosition(wifiLocation); + + if (lastWifiLocation != null && !lastWifiLocation.equals(wifiLocation)) { + List wifiPoints = new ArrayList<>(wifiPolyline.getPoints()); + wifiPoints.add(wifiLocation); + wifiPolyline.setPoints(wifiPoints); + } + lastWifiLocation = wifiLocation; + } + + // Add color-coded observation dot + addObservationDot(wifiObsDots, wifiLocation, + Color.argb(180, 0, 200, 0)); // green + } + + /** + * Remove WiFi marker if user toggles it off + */ + public void clearWiFi() { + if (wifiMarker != null) { + wifiMarker.remove(); + wifiMarker = null; + } + } + + /** + * Whether user is currently showing WiFi or not + */ + public boolean isWifiEnabled() { + return isWifiOn; + } + + /** + * Whether the smooth filter (particle fusion) is enabled. + * When true, display fused position; when false, display raw PDR. + */ + public boolean isSmoothEnabled() { + return isSmoothOn; + } + + /** + * Adds a GNSS observation dot to the map (called each GNSS update). + */ + public void addGnssObservationDot(@NonNull LatLng location) { + if (gMap == null) return; + addObservationDot(gnssObsDots, location, + Color.argb(180, 0, 120, 255)); // blue + } + + /** + * Adds a colored circle dot to the map for a position observation. + * Keeps at most MAX_OBSERVATION_DOTS per source, removing the oldest. + */ + private void addObservationDot(List dotList, LatLng position, int color) { + Circle dot = gMap.addCircle(new CircleOptions() + .center(position) + .radius(1.5) // ~1.5 meter radius + .strokeWidth(1f) + .strokeColor(color) + .fillColor(color) + .zIndex(2)); + dotList.add(dot); + + // Remove oldest dots beyond the limit + while (dotList.size() > MAX_OBSERVATION_DOTS) { + dotList.remove(0).remove(); + } + } + private void setFloorControlsVisibility(int visibility) { floorUpButton.setVisibility(visibility); floorDownButton.setVisibility(visibility); @@ -484,14 +654,20 @@ public void clearMapAndReset() { if (autoFloorSwitch != null) { autoFloorSwitch.setChecked(false); } - if (polyline != null) { - polyline.remove(); - polyline = null; + for (List segs : floorPolylines.values()) { + for (Polyline p : segs) p.remove(); } + floorPolylines.clear(); + activePolyline = null; + polylineFloor = -1; if (gnssPolyline != null) { gnssPolyline.remove(); gnssPolyline = null; } + if (wifiPolyline != null) { + wifiPolyline.remove(); + wifiPolyline = null; + } if (orientationMarker != null) { orientationMarker.remove(); orientationMarker = null; @@ -500,26 +676,36 @@ public void clearMapAndReset() { gnssMarker.remove(); gnssMarker = null; } + if (wifiMarker != null) { + wifiMarker.remove(); + wifiMarker = null; + } lastGnssLocation = null; + lastWifiLocation = null; currentLocation = null; + // Clear observation dots + for (Circle c : gnssObsDots) c.remove(); + gnssObsDots.clear(); + for (Circle c : wifiObsDots) c.remove(); + wifiObsDots.clear(); + // Clear test point markers for (Marker m : testPointMarkers) { m.remove(); } testPointMarkers.clear(); - - // Re-create empty polylines with your chosen colors + // Re-create empty GNSS/WiFi polylines (PDR polyline is per-floor, created on demand) if (gMap != null) { - polyline = gMap.addPolyline(new PolylineOptions() - .color(Color.RED) - .width(5f) - .add()); gnssPolyline = gMap.addPolyline(new PolylineOptions() .color(Color.BLUE) .width(5f) .add()); + wifiPolyline = gMap.addPolyline(new PolylineOptions() + .color(Color.GREEN) + .width(5f) + .add()); } } @@ -615,6 +801,64 @@ private void drawBuildingPolygon() { Log.d(TAG, "Building polygon added, vertex count: " + buildingPolygon.getPoints().size()); } + //region Per-floor polyline management + + /** + * Creates a new polyline segment on the given floor. Hides segments from + * other floors and shows segments for the target floor. + * + * @param floorIdx the floor index to start drawing on + * @param startPoint the first point of the new segment (continuity anchor) + */ + private void startNewPolylineSegment(int floorIdx, LatLng startPoint) { + if (gMap == null) return; + + // Hide old floor's segments, show new floor's segments + if (floorIdx != polylineFloor) { + setFloorPolylinesVisible(polylineFloor, false); + setFloorPolylinesVisible(floorIdx, true); + Log.e(TAG, "POLYLINE floor switch: " + polylineFloor + " -> " + floorIdx); + } + + polylineFloor = floorIdx; + + // Create a new segment starting at the current point + activePolyline = gMap.addPolyline(new PolylineOptions() + .color(isRed ? Color.RED : Color.BLACK) + .width(5f) + .add(startPoint)); + + List segs = floorPolylines.get(floorIdx); + if (segs == null) { + segs = new ArrayList<>(); + floorPolylines.put(floorIdx, segs); + } + segs.add(activePolyline); + } + + /** + * Shows or hides all polyline segments for a given floor. + */ + private void setFloorPolylinesVisible(int floorIdx, boolean visible) { + List segs = floorPolylines.get(floorIdx); + if (segs == null) return; + for (Polyline p : segs) { + p.setVisible(visible); + } + } + + /** + * Called by IndoorMapManager (via evaluateAutoFloor) when the displayed floor + * changes. Toggles polyline visibility so only the current floor's path shows. + */ + public void onFloorChanged(int newFloorIdx) { + if (newFloorIdx == polylineFloor) return; + setFloorPolylinesVisible(polylineFloor, false); + setFloorPolylinesVisible(newFloorIdx, true); + } + + //endregion + //region Auto-floor logic /** @@ -653,16 +897,37 @@ private void applyImmediateFloor() { if (!indoorMapManager.getIsIndoorMapSet()) return; int candidateFloor; - if (sensorFusion.getLatLngWifiPositioning() != null) { + String source; + + // Priority 1: barometric elevation with 70% hysteresis threshold + float elevation = sensorFusion.getElevation(); + float floorHeight = indoorMapManager.getFloorHeight(); + if (floorHeight > 0) { + float ratio = elevation / floorHeight; + int lowerFloor = (int) Math.floor(ratio); + float frac = ratio - lowerFloor; + if (frac >= 0.7f) { + candidateFloor = lowerFloor + 1; + } else { + candidateFloor = lowerFloor; + } + source = "Baro(elev=" + String.format("%.1f", elevation) + + ",height=" + String.format("%.1f", floorHeight) + + ",ratio=" + String.format("%.2f", ratio) + + ",frac=" + String.format("%.2f", frac) + ")"; + } else if (sensorFusion.getLatLngWifiPositioning() != null) { candidateFloor = sensorFusion.getWifiFloor(); + source = "WiFi(floor=" + candidateFloor + ")"; } else { - float elevation = sensorFusion.getElevation(); - float floorHeight = indoorMapManager.getFloorHeight(); - if (floorHeight <= 0) return; - candidateFloor = Math.round(elevation / floorHeight); + return; } + Log.e(TAG, "AUTO_FLOOR immediate: candidate=" + candidateFloor + + " | source=" + source + + " | bias=" + indoorMapManager.getAutoFloorBias()); + indoorMapManager.setCurrentFloor(candidateFloor, true); + onFloorChanged(indoorMapManager.getCurrentFloor()); updateFloorLabel(); // Seed the debounce state so subsequent checks don't re-trigger immediately lastCandidateFloor = candidateFloor; @@ -691,28 +956,78 @@ private void evaluateAutoFloor() { if (!indoorMapManager.getIsIndoorMapSet()) return; int candidateFloor; - - // Priority 1: WiFi-based floor (only if WiFi positioning has returned data) - if (sensorFusion.getLatLngWifiPositioning() != null) { + String source; + + // Priority 1: barometric elevation (responds in seconds to floor changes) + // Uses 70% hysteresis threshold to prevent oscillation at floor boundaries. + // With Math.round() (50% threshold), barometric noise causes constant + // floor flipping when elevation hovers near a floor boundary (e.g. on stairs). + float elevation = sensorFusion.getElevation(); + float floorHeight = indoorMapManager.getFloorHeight(); + if (floorHeight > 0) { + float ratio = elevation / floorHeight; + int lowerFloor = (int) Math.floor(ratio); + float frac = ratio - lowerFloor; + // Only assign to upper floor when clearly past 70% of floor height; + // this creates a dead zone (30%-70%) that prevents oscillation. + if (frac >= 0.7f) { + candidateFloor = lowerFloor + 1; + } else { + candidateFloor = lowerFloor; + } + source = "Baro(elev=" + String.format("%.1f", elevation) + + ",height=" + String.format("%.1f", floorHeight) + + ",ratio=" + String.format("%.2f", ratio) + + ",frac=" + String.format("%.2f", frac) + ")"; + } else if (sensorFusion.getLatLngWifiPositioning() != null) { + // Fallback: WiFi floor (slower to update but works without barometer) candidateFloor = sensorFusion.getWifiFloor(); + source = "WiFi(floor=" + candidateFloor + ")"; } else { - // Fallback: barometric elevation estimate - float elevation = sensorFusion.getElevation(); - float floorHeight = indoorMapManager.getFloorHeight(); - if (floorHeight <= 0) return; - candidateFloor = Math.round(elevation / floorHeight); + return; } // Debounce: require the same floor reading for AUTO_FLOOR_DEBOUNCE_MS long now = SystemClock.elapsedRealtime(); if (candidateFloor != lastCandidateFloor) { + Log.e(TAG, "AUTO_FLOOR candidate changed: " + lastCandidateFloor + + " -> " + candidateFloor + " | source=" + source + + " | bias=" + indoorMapManager.getAutoFloorBias() + + " | debounce reset"); lastCandidateFloor = candidateFloor; lastCandidateTime = now; return; } if (now - lastCandidateTime >= AUTO_FLOOR_DEBOUNCE_MS) { + // Floor-change gate: only allow near stairs or lift + int targetIndex = candidateFloor + indoorMapManager.getAutoFloorBias(); + int currentIndex = indoorMapManager.getCurrentFloor(); + if (targetIndex != currentIndex) { + String transport = indoorMapManager.getNearbyVerticalTransport(currentLocation); + if (transport == null) { + Log.e(TAG, "AUTO_FLOOR BLOCKED: not near stairs/lift" + + " | candidate=" + candidateFloor + + " | targetIdx=" + targetIndex + + " | currentIdx=" + currentIndex + + " | source=" + source); + // Don't reset timer — keep checking each cycle + return; + } + boolean isElevator = sensorFusion.getElevator(); + String motionType = isElevator ? "elevator" : "walking/stairs"; + Log.e(TAG, "AUTO_FLOOR gate PASSED: near " + transport + + " | motionType=" + motionType + + " | candidate=" + candidateFloor + + " | " + currentIndex + " -> " + targetIndex); + } + + Log.e(TAG, "AUTO_FLOOR applied: candidate=" + candidateFloor + + " | source=" + source + + " | finalIndex=" + targetIndex + + " | display=" + indoorMapManager.getCurrentFloorDisplayName()); indoorMapManager.setCurrentFloor(candidateFloor, true); + onFloorChanged(indoorMapManager.getCurrentFloor()); updateFloorLabel(); // Reset timer so we don't keep re-applying the same floor lastCandidateTime = now; diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/GNSSDataProcessor.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/GNSSDataProcessor.java index 579e344c..996955ff 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/GNSSDataProcessor.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/GNSSDataProcessor.java @@ -6,6 +6,7 @@ import android.content.pm.PackageManager; import android.location.LocationListener; import android.location.LocationManager; +import android.util.Log; import android.widget.Toast; import androidx.core.app.ActivityCompat; @@ -20,6 +21,8 @@ * @author Mate Stodulka */ public class GNSSDataProcessor { + private static final String TAG = "GNSS_DEBUG"; + // Application context for handling permissions and locationManager instances private final Context context; // Locations manager to enable access to GNSS and cellular location data via the android system @@ -49,21 +52,32 @@ public GNSSDataProcessor(Context context, LocationListener locationListener) { // Check for permissions boolean permissionsGranted = checkLocationPermissions(); + Log.e(TAG, "Location permissions granted: " + permissionsGranted); //Location manager and listener this.locationManager = (LocationManager) context.getSystemService(Context.LOCATION_SERVICE); this.locationListener = locationListener; + boolean gpsEnabled = locationManager.isProviderEnabled(LocationManager.GPS_PROVIDER); + boolean networkEnabled = locationManager.isProviderEnabled(LocationManager.NETWORK_PROVIDER); + Log.e(TAG, "GPS provider enabled: " + gpsEnabled); + Log.e(TAG, "Network provider enabled: " + networkEnabled); + // Turn on gps if it is currently disabled - if (!locationManager.isProviderEnabled(LocationManager.GPS_PROVIDER)) { + if (!gpsEnabled) { + Log.e(TAG, "WARNING: GPS provider is DISABLED"); Toast.makeText(context, "Open GPS", Toast.LENGTH_SHORT).show(); } - if (!locationManager.isProviderEnabled(LocationManager.NETWORK_PROVIDER)) { + if (!networkEnabled) { + Log.e(TAG, "WARNING: Network provider is DISABLED"); Toast.makeText(context, "Enable Cellular", Toast.LENGTH_SHORT).show(); } // Start location updates if (permissionsGranted) { + Log.e(TAG, "Starting location updates from GPS + Network providers"); startLocationUpdates(); + } else { + Log.e(TAG, "WARNING: Cannot start location updates - permissions not granted"); } } @@ -98,18 +112,23 @@ private boolean checkLocationPermissions() { */ @SuppressLint("MissingPermission") public void startLocationUpdates() { - //if (sharedPreferences.getBoolean("location", true)) { boolean permissionGranted = checkLocationPermissions(); - if (permissionGranted && locationManager.isProviderEnabled(LocationManager.GPS_PROVIDER) && - locationManager.isProviderEnabled(LocationManager.NETWORK_PROVIDER)){ + boolean gpsOn = locationManager.isProviderEnabled(LocationManager.GPS_PROVIDER); + boolean netOn = locationManager.isProviderEnabled(LocationManager.NETWORK_PROVIDER); + Log.e(TAG, "startLocationUpdates() - permission=" + permissionGranted + + " GPS=" + gpsOn + " Network=" + netOn); + if (permissionGranted && gpsOn && netOn) { locationManager.requestLocationUpdates(LocationManager.GPS_PROVIDER, 0, 0, locationListener); locationManager.requestLocationUpdates(LocationManager.NETWORK_PROVIDER, 0, 0, locationListener); + Log.e(TAG, "Registered for GPS + Network location updates (minTime=0, minDist=0)"); } - else if(permissionGranted && !locationManager.isProviderEnabled(LocationManager.GPS_PROVIDER)){ + else if(permissionGranted && !gpsOn){ + Log.e(TAG, "WARNING: GPS provider not available, cannot register"); Toast.makeText(context, "Open GPS", Toast.LENGTH_LONG).show(); } - else if(permissionGranted && !locationManager.isProviderEnabled(LocationManager.NETWORK_PROVIDER)){ + else if(permissionGranted && !netOn){ + Log.e(TAG, "WARNING: Network provider not available, cannot register"); Toast.makeText(context, "Turn on WiFi", Toast.LENGTH_LONG).show(); } } @@ -118,6 +137,7 @@ else if(permissionGranted && !locationManager.isProviderEnabled(LocationManager. * Stops updates to the location listener via the location manager. */ public void stopUpdating() { + Log.e(TAG, "Stopping GNSS location updates"); locationManager.removeUpdates(locationListener); } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java index 639fc5c2..b916c43b 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -22,6 +22,7 @@ */ public class SensorEventHandler { + private static final String TAG = "HEADING_DEBUG"; private static final float ALPHA = 0.8f; private static final long LARGE_GAP_THRESHOLD_MS = 500; @@ -29,6 +30,7 @@ public class SensorEventHandler { private final PdrProcessing pdrProcessing; private final PathView pathView; private final TrajectoryRecorder recorder; + private final PositionFusion positionFusion; // Timestamp tracking private final HashMap lastEventTimestamps = new HashMap<>(); @@ -39,22 +41,36 @@ public class SensorEventHandler { // Acceleration magnitude buffer between steps private final List accelMagnitude = new ArrayList<>(); + // ===================== Software Step Detection ===================== + /** Acceleration magnitude threshold to detect a step peak. */ + private static final double STEP_THRESHOLD = 2.05; + /** Minimum interval between two steps in milliseconds. */ + private static final long MIN_STEP_INTERVAL_MS = 300; + /** Whether the acceleration is currently above the threshold (rising phase). */ + private boolean aboveThreshold = false; + + // Previous PDR position for computing delta to feed into fusion + private float prevPdrX = 0; + private float prevPdrY = 0; + /** * Creates a new SensorEventHandler. * - * @param state shared sensor state holder - * @param pdrProcessing PDR processor for step-length and position calculation - * @param pathView path drawing view for trajectory visualisation - * @param recorder trajectory recorder for checking recording state and writing PDR data - * @param bootTime initial boot time offset + * @param state shared sensor state holder + * @param pdrProcessing PDR processor for step-length and position calculation + * @param pathView path drawing view for trajectory visualisation + * @param recorder trajectory recorder for checking recording state and writing PDR data + * @param positionFusion position fusion engine for multi-source fusion + * @param bootTime initial boot time offset */ public SensorEventHandler(SensorState state, PdrProcessing pdrProcessing, PathView pathView, TrajectoryRecorder recorder, - long bootTime) { + PositionFusion positionFusion, long bootTime) { this.state = state; this.pdrProcessing = pdrProcessing; this.pathView = pathView; this.recorder = recorder; + this.positionFusion = positionFusion; this.bootTime = bootTime; } @@ -92,12 +108,11 @@ public void handleSensorEvent(SensorEvent sensorEvent) { } break; - // NOTE: intentional fall-through from GYROSCOPE to LINEAR_ACCELERATION - // (existing behavior preserved during refactoring) case Sensor.TYPE_GYROSCOPE: state.angularVelocity[0] = sensorEvent.values[0]; state.angularVelocity[1] = sensorEvent.values[1]; state.angularVelocity[2] = sensorEvent.values[2]; + break; case Sensor.TYPE_LINEAR_ACCELERATION: state.filteredAcc[0] = sensorEvent.values[0]; @@ -113,6 +128,19 @@ public void handleSensorEvent(SensorEvent sensorEvent) { state.elevator = pdrProcessing.estimateElevator( state.gravity, state.filteredAcc); + + // Software step detection: detect peak (rising above threshold then falling below) + if (!aboveThreshold && accelMagFiltered > STEP_THRESHOLD) { + aboveThreshold = true; + } else if (aboveThreshold && accelMagFiltered < STEP_THRESHOLD) { + aboveThreshold = false; + // Peak detected — trigger step if enough time has passed + if (currentTime - lastStepTime >= MIN_STEP_INTERVAL_MS + && accelMagnitude.size() >= 2) { + lastStepTime = currentTime; + processStep(SystemClock.uptimeMillis() - bootTime, currentTime); + } + } break; case Sensor.TYPE_GRAVITY: @@ -143,45 +171,135 @@ public void handleSensorEvent(SensorEvent sensorEvent) { float[] rotationVectorDCM = new float[9]; SensorManager.getRotationMatrixFromVector(rotationVectorDCM, state.rotation); SensorManager.getOrientation(rotationVectorDCM, state.orientation); - break; + // Log heading periodically (every ~50th event to avoid spam) + int rotCount = eventCounts.getOrDefault(Sensor.TYPE_ROTATION_VECTOR, 0); + if (rotCount % 50 == 0) { + float magAzDeg = (float) Math.toDegrees(state.orientation[0]); + float gameAzDeg = (float) Math.toDegrees(state.gameOrientation[0]); + float delta = magAzDeg - gameAzDeg; + // Normalize delta to [-180, 180] + if (delta > 180) delta -= 360; + if (delta < -180) delta += 360; + + Log.e(TAG, "Heading: azimuth=" + String.format("%.1f", magAzDeg) + + "deg | pitch=" + String.format("%.1f", Math.toDegrees(state.orientation[1])) + + "deg | roll=" + String.format("%.1f", Math.toDegrees(state.orientation[2])) + + "deg | rotVec=[" + String.format("%.3f,%.3f,%.3f", + state.rotation[0], state.rotation[1], state.rotation[2]) + "]"); + Log.e("MAG_DIAG", "magHeading=" + String.format("%.1f", magAzDeg) + + "deg | gameHeading=" + String.format("%.1f", gameAzDeg) + + "deg | delta=" + String.format("%.1f", delta) + "deg" + + (Math.abs(delta) > 15 ? " *** MAGNETIC INTERFERENCE ***" : "")); + + // Log raw magnetometer magnitude for anomaly detection + float magMagnitude = (float) Math.sqrt( + state.magneticField[0] * state.magneticField[0] + + state.magneticField[1] * state.magneticField[1] + + state.magneticField[2] * state.magneticField[2]); + Log.e("MAG_DIAG", "magField=[" + String.format("%.1f,%.1f,%.1f", + state.magneticField[0], state.magneticField[1], state.magneticField[2]) + + "] magnitude=" + String.format("%.1f", magMagnitude) + "uT" + + (magMagnitude > 100 ? " *** ABNORMAL ***" : "")); + + Log.e("MAG_DIAG", "calibrated=" + state.headingCalibrated + + " | offset=" + String.format("%.1f", Math.toDegrees(state.headingOffset)) + "deg" + + " | finalHeading=" + String.format("%.1f", + Math.toDegrees(state.gameOrientation[0] + state.headingOffset)) + "deg"); + } - case Sensor.TYPE_STEP_DETECTOR: - long stepTime = SystemClock.uptimeMillis() - bootTime; - - if (currentTime - lastStepTime < 20) { - Log.e("SensorFusion", "Ignoring step event, too soon after last step event:" - + (currentTime - lastStepTime) + " ms"); - break; - } else { - lastStepTime = currentTime; - - if (accelMagnitude.isEmpty()) { - Log.e("SensorFusion", - "stepDetection triggered, but accelMagnitude is empty! " + - "This can cause updatePdr(...) to fail or return bad results."); - } else { - Log.d("SensorFusion", - "stepDetection triggered, accelMagnitude size = " - + accelMagnitude.size()); + // Auto-calibrate heading offset: mag heading → game rotation offset + float magMag = (float) Math.sqrt( + state.magneticField[0] * state.magneticField[0] + + state.magneticField[1] * state.magneticField[1] + + state.magneticField[2] * state.magneticField[2]); + if (magMag > 5) { // Need at least some magnetic data + float newOffset = state.orientation[0] - state.gameOrientation[0]; + // Normalize to [-PI, PI] + while (newOffset > Math.PI) newOffset -= 2 * Math.PI; + while (newOffset < -Math.PI) newOffset += 2 * Math.PI; + + boolean magNormal = (magMag > 25 && magMag < 80); + + if (!state.headingCalibrated) { + // First calibration: use whatever mag heading is available + state.headingOffset = newOffset; + state.headingCalibrated = true; + Log.e("MAG_DIAG", "=== HEADING CALIBRATED (initial) === offset=" + + String.format("%.1f", Math.toDegrees(newOffset)) + + "deg | magMagnitude=" + String.format("%.1f", magMag) + "uT" + + (magNormal ? " (GOOD)" : " (NOISY)")); + } else if (magNormal) { + // Refine offset only when magnetic field is normal + float diff = newOffset - state.headingOffset; + while (diff > Math.PI) diff -= 2 * Math.PI; + while (diff < -Math.PI) diff += 2 * Math.PI; + state.headingOffset += 0.05f * diff; } + } + break; - float[] newCords = this.pdrProcessing.updatePdr( - stepTime, - this.accelMagnitude, - state.orientation[0] - ); + case Sensor.TYPE_GAME_ROTATION_VECTOR: + float[] gameRotDCM = new float[9]; + SensorManager.getRotationMatrixFromVector(gameRotDCM, sensorEvent.values); + SensorManager.getOrientation(gameRotDCM, state.gameOrientation); + break; - this.accelMagnitude.clear(); + case Sensor.TYPE_STEP_DETECTOR: + // Hardware step detector disabled — using software peak detection instead + break; + } + } - if (recorder.isRecording()) { - this.pathView.drawTrajectory(newCords); - state.stepCounter++; - recorder.addPdrData( - SystemClock.uptimeMillis() - bootTime, - newCords[0], newCords[1]); - } - break; - } + /** + * Processes a detected step: computes heading, updates PDR, feeds particle filter. + * Called by software peak detection when a step is identified. + * + * @param stepTime elapsed time since boot in milliseconds + * @param currentTime wall-clock time from System.currentTimeMillis() + */ + private void processStep(long stepTime, long currentTime) { + float stepMagAz = (float) Math.toDegrees(state.orientation[0]); + float stepGameAz = (float) Math.toDegrees(state.gameOrientation[0]); + float stepDelta = stepMagAz - stepGameAz; + if (stepDelta > 180) stepDelta -= 360; + if (stepDelta < -180) stepDelta += 360; + + Log.e(TAG, "Step detected (software) | heading(azimuth)=" + + String.format("%.1f", stepMagAz) + "deg" + + " | gameHeading=" + String.format("%.1f", stepGameAz) + "deg" + + " | magDelta=" + String.format("%.1f", stepDelta) + "deg" + + " | accelSamples=" + this.accelMagnitude.size()); + + // Use game rotation + calibration offset for PDR heading + float calibratedHeading = state.gameOrientation[0] + state.headingOffset; + // Normalize to [-PI, PI] to prevent heading drift + while (calibratedHeading > (float) Math.PI) calibratedHeading -= (float) (2 * Math.PI); + while (calibratedHeading < (float) -Math.PI) calibratedHeading += (float) (2 * Math.PI); + + float[] newCords = this.pdrProcessing.updatePdr( + stepTime, + this.accelMagnitude, + calibratedHeading + ); + + this.accelMagnitude.clear(); + + if (recorder.isRecording()) { + this.pathView.drawTrajectory(newCords); + state.stepCounter++; + recorder.addPdrData( + SystemClock.uptimeMillis() - bootTime, + newCords[0], newCords[1]); + + // Compute PDR delta and feed into position fusion + float dEast = newCords[0] - prevPdrX; + float dNorth = newCords[1] - prevPdrY; + prevPdrX = newCords[0]; + prevPdrY = newCords[1]; + + if (positionFusion != null && positionFusion.isInitialized()) { + positionFusion.predictWithPdr(dEast, dNorth, calibratedHeading); + } } } @@ -203,5 +321,8 @@ public void logSensorFrequencies() { */ void resetBootTime(long newBootTime) { this.bootTime = newBootTime; + // Reset PDR delta tracking for new recording + this.prevPdrX = 0; + this.prevPdrY = 0; } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index aeb6386a..efe2a2e6 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -11,6 +11,7 @@ import android.os.Handler; import android.os.Looper; import android.os.SystemClock; +import android.util.Log; import android.widget.Toast; import androidx.annotation.NonNull; @@ -19,6 +20,7 @@ import com.google.android.gms.maps.model.LatLng; import com.openpositioning.PositionMe.data.remote.FloorplanApiClient; import com.openpositioning.PositionMe.presentation.activity.MainActivity; +import com.openpositioning.PositionMe.sensors.PositionFusion; import com.openpositioning.PositionMe.service.SensorCollectionService; import com.openpositioning.PositionMe.utils.PathView; import com.openpositioning.PositionMe.utils.PdrProcessing; @@ -75,6 +77,7 @@ public class SensorFusion implements SensorEventListener { private MovementSensor rotationSensor; private MovementSensor gravitySensor; private MovementSensor linearAccelerationSensor; + private MovementSensor gameRotationSensor; // Non-sensor data sources private WifiDataProcessor wifiProcessor; @@ -88,6 +91,9 @@ public class SensorFusion implements SensorEventListener { private PdrProcessing pdrProcessing; private PathView pathView; + // Position fusion engine + private PositionFusion positionFusion; + // Sensor registration latency setting long maxReportLatencyNs = 0; @@ -141,6 +147,7 @@ public void setContext(Context context) { this.rotationSensor = new MovementSensor(context, Sensor.TYPE_ROTATION_VECTOR); this.gravitySensor = new MovementSensor(context, Sensor.TYPE_GRAVITY); this.linearAccelerationSensor = new MovementSensor(context, Sensor.TYPE_LINEAR_ACCELERATION); + this.gameRotationSensor = new MovementSensor(context, Sensor.TYPE_GAME_ROTATION_VECTOR); // Initialise non-sensor data sources this.gnssProcessor = new GNSSDataProcessor(context, locationListener); @@ -152,17 +159,20 @@ public void setContext(Context context) { this.pathView = new PathView(context, null); WiFiPositioning wiFiPositioning = new WiFiPositioning(context); + // Create position fusion engine + this.positionFusion = new PositionFusion(); + // Create internal modules this.recorder = new TrajectoryRecorder(appContext, state, serverCommunications, settings); this.recorder.setSensorReferences( accelerometerSensor, gyroscopeSensor, magnetometerSensor, barometerSensor, lightSensor, proximitySensor, rotationSensor); - this.wifiPositionManager = new WifiPositionManager(wiFiPositioning, recorder); + this.wifiPositionManager = new WifiPositionManager(wiFiPositioning, recorder, positionFusion); long bootTime = SystemClock.uptimeMillis(); this.eventHandler = new SensorEventHandler( - state, pdrProcessing, pathView, recorder, bootTime); + state, pdrProcessing, pathView, recorder, positionFusion, bootTime); // Register WiFi observer on WifiPositionManager (not on SensorFusion) this.wifiProcessor = new WifiDataProcessor(context); @@ -243,6 +253,10 @@ public void resumeListening() { stepDetectionSensor.sensor, SensorManager.SENSOR_DELAY_NORMAL); rotationSensor.sensorManager.registerListener(this, rotationSensor.sensor, (int) 1e6); + if (gameRotationSensor.sensor != null) { + gameRotationSensor.sensorManager.registerListener(this, + gameRotationSensor.sensor, (int) 1e6); + } // Foreground service owns WiFi/BLE scanning during recording. if (!recorder.isRecording()) { startWirelessCollectors(); @@ -329,6 +343,13 @@ public void startRecording() { recorder.startRecording(pdrProcessing); eventHandler.resetBootTime(recorder.getBootTime()); + // Initialize position fusion with the user-selected start position + if (state.startLocation[0] != 0 || state.startLocation[1] != 0) { + positionFusion.init(state.startLocation[0], state.startLocation[1]); + } else { + Log.e("FUSION_DEBUG", "WARNING: startRecording called but startLocation is (0,0)"); + } + // Handover WiFi/BLE scan lifecycle from activity callbacks to foreground service. stopWirelessCollectors(); @@ -346,6 +367,7 @@ public void startRecording() { */ public void stopRecording() { recorder.stopRecording(); + positionFusion.reset(); if (appContext != null) { SensorCollectionService.stop(appContext); } @@ -511,12 +533,12 @@ public float passAverageStepLength() { } /** - * Getter function for device orientation. + * Getter function for device orientation (game rotation + calibration offset). * - * @return orientation of device in radians. + * @return calibrated orientation of device in radians. */ public float passOrientation() { - return state.orientation[0]; + return state.gameOrientation[0] + state.headingOffset; } /** @@ -623,6 +645,24 @@ public int getWifiFloor() { return wifiPositionManager.getWifiFloor(); } + /** + * Returns the fused position combining PDR, GNSS and WiFi. + * + * @return fused LatLng, or null if fusion not yet initialized + */ + public LatLng getFusedPosition() { + return positionFusion.getFusedLatLng(); + } + + /** + * Returns the position fusion engine instance. + * + * @return the PositionFusion instance + */ + public PositionFusion getPositionFusion() { + return positionFusion; + } + /** * Utility function to log the event frequency of each sensor. */ @@ -640,11 +680,65 @@ public void logSensorFrequencies() { * {@link TrajectoryRecorder}. */ class MyLocationListener implements LocationListener { + private static final String TAG = "GNSS_DEBUG"; + @Override public void onLocationChanged(@NonNull Location location) { - state.latitude = (float) location.getLatitude(); - state.longitude = (float) location.getLongitude(); + float lat = (float) location.getLatitude(); + float lng = (float) location.getLongitude(); + float accuracy = location.getAccuracy(); + String provider = location.getProvider(); + float speed = location.getSpeed(); + double altitude = location.getAltitude(); + long time = location.getTime(); + + Log.e(TAG, "=== GNSS Location Update ==="); + Log.e(TAG, "Provider: " + provider + + " | lat: " + lat + + " | lng: " + lng + + " | accuracy: " + accuracy + "m" + + " | speed: " + speed + "m/s" + + " | altitude: " + altitude + "m" + + " | time: " + time); + + // Flag suspicious readings + if (accuracy > 50) { + Log.e(TAG, "WARNING: GNSS accuracy very poor (>" + accuracy + "m), provider=" + provider); + } + + // Check for large jumps from previous position + if (state.latitude != 0 && state.longitude != 0) { + double dLat = Math.abs(lat - state.latitude); + double dLng = Math.abs(lng - state.longitude); + double jumpMeters = Math.sqrt( + Math.pow(dLat * 111111, 2) + + Math.pow(dLng * 111111 * Math.cos(Math.toRadians(lat)), 2)); + if (jumpMeters > 100) { + Log.e(TAG, "WARNING: GNSS position jumped " + String.format("%.1f", jumpMeters) + + "m from previous (" + + state.latitude + "," + state.longitude + + ") -> (" + lat + "," + lng + ")"); + } + Log.e(TAG, "Delta from prev: " + String.format("%.2f", jumpMeters) + "m"); + } + + // Check distance from Nucleus building center (55.9230, -3.1743) + double distFromNucleus = Math.sqrt( + Math.pow((lat - 55.9230) * 111111, 2) + + Math.pow((lng - (-3.1743)) * 111111 * Math.cos(Math.toRadians(55.9230)), 2)); + Log.e(TAG, "Distance from Nucleus center: " + String.format("%.1f", distFromNucleus) + "m"); + if (distFromNucleus > 500) { + Log.e(TAG, "WARNING: GNSS position is >" + String.format("%.0f", distFromNucleus) + "m from building!"); + } + + state.latitude = lat; + state.longitude = lng; recorder.addGnssData(location); + + // Feed GNSS observation into position fusion + if (positionFusion.isInitialized()) { + positionFusion.updateWithGnss(lat, lng, accuracy); + } } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java index 1554f61d..553472c3 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java @@ -23,6 +23,14 @@ public class SensorState { // Rotation vector: volatile because TYPE_ROTATION_VECTOR replaces via clone() public volatile float[] rotation = new float[]{0, 0, 0, 1.0f}; + // Game rotation vector (no magnetometer) for diagnostic comparison + public final float[] gameOrientation = new float[3]; + + // Heading calibration: offset from game rotation to magnetic north (radians) + // calibratedHeading = gameOrientation[0] + headingOffset + public volatile float headingOffset = 0f; + public volatile boolean headingCalibrated = false; + // Scalar sensors public volatile float pressure; public volatile float light; diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java index dbf809dd..05286ad8 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java @@ -28,6 +28,8 @@ * @author Arun Gopalakrishnan */ public class WiFiPositioning { + private static final String TAG = "WIFI_POS_DEBUG"; + // Queue for storing the POST requests made private RequestQueue requestQueue; // URL for WiFi positioning API @@ -81,33 +83,61 @@ public WiFiPositioning(Context context){ * @param jsonWifiFeatures WiFi Fingerprint from device */ public void request(JSONObject jsonWifiFeatures) { + Log.e(TAG, "=== WiFi Position Request (no callback) ==="); + Log.e(TAG, "Request URL: " + url); + Log.e(TAG, "Fingerprint payload: " + jsonWifiFeatures.toString()); + long requestTime = System.currentTimeMillis(); + // Creating the POST request using WiFi fingerprint (a JSON object) JsonObjectRequest jsonObjectRequest = new JsonObjectRequest( Request.Method.POST, url, jsonWifiFeatures, // Parses the response to obtain the WiFi location and WiFi floor response -> { + long responseTime = System.currentTimeMillis() - requestTime; + Log.e(TAG, "WiFi API response received in " + responseTime + "ms"); + Log.e(TAG, "Raw response: " + response.toString()); try { - wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon")); - floor = response.getInt("floor"); + double lat = response.getDouble("lat"); + double lon = response.getDouble("lon"); + int floorVal = response.getInt("floor"); + wifiLocation = new LatLng(lat, lon); + floor = floorVal; + Log.e(TAG, "WiFi position: lat=" + lat + " lon=" + lon + " floor=" + floorVal); + + // Check distance from Nucleus building center + double distFromNucleus = Math.sqrt( + Math.pow((lat - 55.9230) * 111111, 2) + + Math.pow((lon - (-3.1743)) * 111111 * Math.cos(Math.toRadians(55.9230)), 2)); + Log.e(TAG, "WiFi pos distance from Nucleus: " + String.format("%.1f", distFromNucleus) + "m"); + if (distFromNucleus > 500) { + Log.e(TAG, "WARNING: WiFi position is far from building! Possible bad fingerprint"); + } } catch (JSONException e) { - // Error log to keep record of errors (for secure programming and maintainability) - Log.e("jsonErrors","Error parsing response: "+e.getMessage()+" "+ response); + Log.e(TAG,"Error parsing response: "+e.getMessage()+" "+ response); } }, // Handles the errors obtained from the POST request error -> { + long responseTime = System.currentTimeMillis() - requestTime; + Log.e(TAG, "WiFi API ERROR after " + responseTime + "ms"); // Validation Error if (error.networkResponse!=null && error.networkResponse.statusCode==422){ - Log.e("WiFiPositioning", "Validation Error "+ error.getMessage()); + Log.e(TAG, "Validation Error (422): " + error.getMessage()); + if (error.networkResponse.data != null) { + Log.e(TAG, "Response body: " + new String(error.networkResponse.data)); + } } // Other Errors else{ - // When Response code is available if (error.networkResponse!=null) { - Log.e("WiFiPositioning","Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage()); + Log.e(TAG,"HTTP " + error.networkResponse.statusCode + ": " + error.getMessage()); + if (error.networkResponse.data != null) { + Log.e(TAG, "Response body: " + new String(error.networkResponse.data)); + } } else{ - Log.e("WiFiPositioning","Error message: " + error.getMessage()); + Log.e(TAG,"Network error (no response): " + error.getMessage() + + " | Cause: " + (error.getCause() != null ? error.getCause().toString() : "null")); } } } @@ -132,38 +162,40 @@ public void request(JSONObject jsonWifiFeatures) { * @param callback callback function to allow user to use location when ready */ public void request( JSONObject jsonWifiFeatures, final VolleyCallback callback) { + Log.e(TAG, "=== WiFi Position Request (with callback) ==="); + Log.e(TAG, "Fingerprint payload: " + jsonWifiFeatures.toString()); + long requestTime = System.currentTimeMillis(); + // Creating the POST request using WiFi fingerprint (a JSON object) JsonObjectRequest jsonObjectRequest = new JsonObjectRequest( Request.Method.POST, url, jsonWifiFeatures, response -> { + long responseTime = System.currentTimeMillis() - requestTime; + Log.e(TAG, "WiFi API (callback) response in " + responseTime + "ms: " + response.toString()); try { - Log.d("jsonObject",response.toString()); - wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon")); - floor = response.getInt("floor"); - callback.onSuccess(wifiLocation,floor); + double lat = response.getDouble("lat"); + double lon = response.getDouble("lon"); + int floorVal = response.getInt("floor"); + wifiLocation = new LatLng(lat, lon); + floor = floorVal; + Log.e(TAG, "WiFi position (callback): lat=" + lat + " lon=" + lon + " floor=" + floorVal); + callback.onSuccess(wifiLocation, floor); } catch (JSONException e) { - Log.e("jsonErrors","Error parsing response: "+e.getMessage()+" "+ response); + Log.e(TAG,"Error parsing response: "+e.getMessage()+" "+ response); callback.onError("Error parsing response: " + e.getMessage()); } }, error -> { - // Validation Error - if (error.networkResponse!=null && error.networkResponse.statusCode==422){ - Log.e("WiFiPositioning", "Validation Error "+ error.getMessage()); - callback.onError( "Validation Error (422): "+ error.getMessage()); - } - // Other Errors - else{ - // When Response code is available - if (error.networkResponse!=null) { - Log.e("WiFiPositioning","Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage()); - callback.onError("Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage()); - } - else{ - Log.e("WiFiPositioning","Error message: " + error.getMessage()); - callback.onError("Error message: " + error.getMessage()); - } + long responseTime = System.currentTimeMillis() - requestTime; + int statusCode = (error.networkResponse != null) + ? error.networkResponse.statusCode : -1; + Log.e(TAG, "WiFi API (callback) ERROR after " + responseTime + + "ms, HTTP " + statusCode + ": " + error.getMessage()); + if (error.networkResponse != null && error.networkResponse.data != null) { + Log.e(TAG, "Response body: " + new String(error.networkResponse.data)); } + // Pass status code in error message so caller can detect 404 + callback.onError("HTTP " + statusCode + ": " + error.getMessage()); } ); // Adds the request to the request queue diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiDataProcessor.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiDataProcessor.java index 9143c7a1..eab52e37 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiDataProcessor.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiDataProcessor.java @@ -9,8 +9,10 @@ import android.net.ConnectivityManager; import android.net.NetworkInfo; import android.net.wifi.ScanResult; +import android.net.wifi.WifiInfo; import android.net.wifi.WifiManager; import android.provider.Settings; +import android.util.Log; import android.widget.Toast; import androidx.core.app.ActivityCompat; @@ -40,6 +42,8 @@ */ public class WifiDataProcessor implements Observable { + private static final String TAG = "WIFI_SCAN_DEBUG"; + //Time over which a new scan will be initiated private static final long scanInterval = 5000; @@ -122,13 +126,32 @@ public WifiDataProcessor(Context context) { public void onReceive(Context context, Intent intent) { if (ActivityCompat.checkSelfPermission(context, Manifest.permission.ACCESS_FINE_LOCATION) != PackageManager.PERMISSION_GRANTED) { - // Unregister this listener + Log.e(TAG, "WiFi scan aborted - FINE_LOCATION permission not granted"); stopListening(); return; } //Collect the list of nearby wifis List wifiScanList = wifiManager.getScanResults(); + Log.e(TAG, "=== WiFi Scan Completed ==="); + Log.e(TAG, "Total scan results: " + wifiScanList.size()); + + // Log current WiFi connection status + WifiInfo connInfo = wifiManager.getConnectionInfo(); + if (connInfo != null) { + Log.e(TAG, "Current WiFi connection: SSID=" + connInfo.getSSID() + + " BSSID=" + connInfo.getBSSID() + + " RSSI=" + connInfo.getRssi() + "dBm" + + " linkSpeed=" + connInfo.getLinkSpeed() + "Mbps" + + " freq=" + connInfo.getFrequency() + "MHz"); + } else { + Log.e(TAG, "No current WiFi connection"); + } + + // Check network connectivity + ConnectivityManager cm = (ConnectivityManager) context.getSystemService(Context.CONNECTIVITY_SERVICE); + NetworkInfo netInfo = cm.getActiveNetworkInfo(); + Log.e(TAG, "Network connectivity: " + (netInfo != null ? netInfo.getTypeName() + " connected=" + netInfo.isConnected() : "NO NETWORK")); //Stop receiver as scan is complete try { context.unregisterReceiver(this); @@ -384,12 +407,16 @@ public Wifi getCurrentWifiData(){ long intMacAddress = convertBssidToLong(wifiMacAddress); currentWifi.setBssid(intMacAddress); currentWifi.setFrequency(wifiManager.getConnectionInfo().getFrequency()); + Log.e(TAG, "Current WiFi: SSID=" + currentWifi.getSsid() + + " BSSID=" + wifiMacAddress + + " freq=" + currentWifi.getFrequency() + "MHz"); } else{ //Store standard information if not connected currentWifi.setSsid("Not connected"); currentWifi.setBssid(0); currentWifi.setFrequency(0); + Log.e(TAG, "WARNING: WiFi not connected - positioning API may fail"); } return currentWifi; } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java index 1edfd68a..b40ebb5c 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -7,6 +7,10 @@ import org.json.JSONException; import org.json.JSONObject; +import java.util.ArrayList; +import java.util.Collections; +import java.util.Comparator; +import java.util.Iterator; import java.util.List; import java.util.stream.Collectors; import java.util.stream.Stream; @@ -17,15 +21,22 @@ *

Implements {@link Observer} to receive updates from {@link WifiDataProcessor}, * replacing the role previously held by {@link SensorFusion}.

* + *

Filters out mobile hotspot APs (randomized MAC addresses) before sending + * fingerprints to the positioning API. If the API returns 404 (no match), + * retries once with the strongest AP removed, as the positioning engine + * uses the top 5 APs for initial search.

+ * * @see WifiDataProcessor the observable that triggers WiFi scan updates * @see WiFiPositioning the API client for WiFi-based positioning */ public class WifiPositionManager implements Observer { + private static final String TAG = "WIFI_POS_DEBUG"; private static final String WIFI_FINGERPRINT = "wf"; private final WiFiPositioning wiFiPositioning; private final TrajectoryRecorder recorder; + private final PositionFusion positionFusion; private List wifiList; /** @@ -33,11 +44,14 @@ public class WifiPositionManager implements Observer { * * @param wiFiPositioning WiFi positioning API client * @param recorder trajectory recorder for writing WiFi fingerprints + * @param positionFusion position fusion engine for multi-source correction */ public WifiPositionManager(WiFiPositioning wiFiPositioning, - TrajectoryRecorder recorder) { + TrajectoryRecorder recorder, + PositionFusion positionFusion) { this.wiFiPositioning = wiFiPositioning; this.recorder = recorder; + this.positionFusion = positionFusion; } /** @@ -50,24 +64,133 @@ public WifiPositionManager(WiFiPositioning wiFiPositioning, @Override public void update(Object[] wifiList) { this.wifiList = Stream.of(wifiList).map(o -> (Wifi) o).collect(Collectors.toList()); + Log.e(TAG, "=== WiFi Scan Update ==="); + Log.e(TAG, "Total APs scanned: " + this.wifiList.size()); + for (int i = 0; i < Math.min(this.wifiList.size(), 5); i++) { + Wifi w = this.wifiList.get(i); + Log.e(TAG, " AP[" + i + "] SSID=" + w.getSsid() + + " BSSID=" + w.getBssidString() + + " RSSI=" + w.getLevel() + "dBm" + + " freq=" + w.getFrequency() + "MHz" + + " RTT=" + w.isRttEnabled()); + } + if (this.wifiList.size() > 5) { + Log.e(TAG, " ... and " + (this.wifiList.size() - 5) + " more APs"); + } recorder.addWifiFingerprint(this.wifiList); createWifiPositioningRequest(); } /** - * Creates a request to obtain a WiFi location for the obtained WiFi fingerprint. + * Checks if a MAC address is locally administered (randomized). + * Mobile hotspots and privacy-enabled devices use randomized MACs where + * the second least significant bit of the first octet is set to 1. + * For example: e6:c2:d7:bb:48:ab -> first octet 0xe6 = 11100110, bit 1 = 1 -> randomized. + * + * @param bssid the MAC address string (e.g. "e6:c2:d7:bb:48:ab") + * @return true if the MAC is locally administered (likely a mobile hotspot) */ - private void createWifiPositioningRequest() { + private boolean isRandomizedMac(String bssid) { + if (bssid == null || bssid.length() < 2) return false; try { - JSONObject wifiAccessPoints = new JSONObject(); - for (Wifi data : this.wifiList) { - wifiAccessPoints.put(String.valueOf(data.getBssid()), data.getLevel()); + int firstOctet = Integer.parseInt(bssid.substring(0, 2), 16); + // Bit 1 of first octet: 1 = locally administered (randomized) + return (firstOctet & 0x02) != 0; + } catch (NumberFormatException e) { + return false; + } + } + + /** + * Builds a fingerprint JSONObject from the WiFi list, filtering out + * mobile hotspot APs that use randomized MAC addresses. + * + * @param skipStrongest if true, also removes the AP with the strongest signal + * (used for 404 retry, as the API searches by top 5 APs) + * @return the fingerprint JSON, or null if no valid APs remain + */ + private JSONObject buildFingerprint(boolean skipStrongest) throws JSONException { + // Filter out randomized MACs (mobile hotspots) + List filtered = new ArrayList<>(); + int removedCount = 0; + for (Wifi data : this.wifiList) { + String bssid = data.getBssidString(); + if (isRandomizedMac(bssid)) { + removedCount++; + Log.e(TAG, " Filtered out randomized MAC: " + bssid + + " RSSI=" + data.getLevel() + "dBm" + + " (likely mobile hotspot)"); + } else { + filtered.add(data); } - JSONObject wifiFingerPrint = new JSONObject(); - wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); - this.wiFiPositioning.request(wifiFingerPrint); + } + if (removedCount > 0) { + Log.e(TAG, "Filtered " + removedCount + " randomized MAC APs, " + + filtered.size() + " APs remaining"); + } + + // Optionally remove the strongest AP (for 404 retry) + if (skipStrongest && !filtered.isEmpty()) { + Wifi strongest = Collections.max(filtered, + Comparator.comparingInt(Wifi::getLevel)); + Log.e(TAG, "Removing strongest AP for retry: BSSID=" + + strongest.getBssidString() + + " RSSI=" + strongest.getLevel() + "dBm"); + filtered.remove(strongest); + } + + if (filtered.isEmpty()) { + Log.e(TAG, "No valid APs remaining after filtering"); + return null; + } + + JSONObject wifiAccessPoints = new JSONObject(); + for (Wifi data : filtered) { + wifiAccessPoints.put(String.valueOf(data.getBssid()), data.getLevel()); + } + JSONObject wifiFingerPrint = new JSONObject(); + wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); + return wifiFingerPrint; + } + + /** + * Creates a WiFi positioning request with mobile hotspot filtering. + * If the API returns 404, retries once with the strongest AP removed. + */ + private void createWifiPositioningRequest() { + try { + JSONObject fingerprint = buildFingerprint(false); + if (fingerprint == null) return; + + Log.e(TAG, "Sending WiFi positioning request with filtered APs"); + this.wiFiPositioning.request(fingerprint, new WiFiPositioning.VolleyCallback() { + @Override + public void onSuccess(LatLng location, int floor) { + Log.e(TAG, "WiFi positioning SUCCESS: " + location + " floor=" + floor); + // Feed WiFi observation into position fusion + if (positionFusion != null && positionFusion.isInitialized()) { + positionFusion.updateWithWifi(location.latitude, location.longitude); + } + } + + @Override + public void onError(String message) { + // If 404 (no match), retry once with strongest AP removed + if (message != null && message.contains("404")) { + Log.e(TAG, "Got 404, retrying with strongest AP removed..."); + try { + JSONObject retryFp = buildFingerprint(true); + if (retryFp != null) { + wiFiPositioning.request(retryFp); + } + } catch (JSONException e) { + Log.e(TAG, "Error building retry fingerprint: " + e); + } + } + } + }); } catch (JSONException e) { - Log.e("jsonErrors", "Error creating json object" + e.toString()); + Log.e(TAG, "Error creating WiFi fingerprint JSON: " + e.toString()); } } @@ -76,13 +199,10 @@ private void createWifiPositioningRequest() { */ private void createWifiPositionRequestCallback() { try { - JSONObject wifiAccessPoints = new JSONObject(); - for (Wifi data : this.wifiList) { - wifiAccessPoints.put(String.valueOf(data.getBssid()), data.getLevel()); - } - JSONObject wifiFingerPrint = new JSONObject(); - wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); - this.wiFiPositioning.request(wifiFingerPrint, new WiFiPositioning.VolleyCallback() { + JSONObject fingerprint = buildFingerprint(false); + if (fingerprint == null) return; + + this.wiFiPositioning.request(fingerprint, new WiFiPositioning.VolleyCallback() { @Override public void onSuccess(LatLng wifiLocation, int floor) { // Handle the success response @@ -94,7 +214,7 @@ public void onError(String message) { } }); } catch (JSONException e) { - Log.e("jsonErrors", "Error creating json object" + e.toString()); + Log.e(TAG, "Error creating WiFi fingerprint JSON: " + e.toString()); } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/BuildingPolygon.java b/app/src/main/java/com/openpositioning/PositionMe/utils/BuildingPolygon.java index 06f48a46..5792995a 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/BuildingPolygon.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/BuildingPolygon.java @@ -20,6 +20,9 @@ public class BuildingPolygon { // North-East and South-West Coordinates for the Kenneth and Murray Library Building public static final LatLng LIBRARY_NE=new LatLng(55.92306692576906, -3.174771893078224); public static final LatLng LIBRARY_SW=new LatLng(55.92281045664704, -3.175184089079065); + // Campus: bounding rectangle covering both Nucleus and Library + public static final LatLng CAMPUS_NE = NUCLEUS_NE; // northernmost + easternmost + public static final LatLng CAMPUS_SW = LIBRARY_SW; // southernmost + westernmost // North-East and South-West Coordinates for Murchison House public static final LatLng MURCHISON_NE=new LatLng(55.92240, -3.17150); public static final LatLng MURCHISON_SW=new LatLng(55.92170, -3.17280); @@ -45,6 +48,13 @@ public class BuildingPolygon { add(BuildingPolygon.MURCHISON_SW); add(new LatLng(BuildingPolygon.MURCHISON_NE.latitude, BuildingPolygon.MURCHISON_SW.longitude)); }}; + // Campus polygon: bounding rectangle covering Nucleus + Library + public static final List CAMPUS_POLYGON = new ArrayList() {{ + add(CAMPUS_NE); + add(new LatLng(CAMPUS_SW.latitude, CAMPUS_NE.longitude)); + add(CAMPUS_SW); + add(new LatLng(CAMPUS_NE.latitude, CAMPUS_SW.longitude)); + }}; /** * Function to check if a point is in the Nucleus Building @@ -73,6 +83,116 @@ public static boolean inMurchison(LatLng point){ return (pointInPolygon(point, MURCHISON_POLYGON)); } + /** + * Expands a polygon outward from its centroid by the given buffer distance. + * Each vertex is moved along the centroid→vertex direction by bufferMeters. + * Used for exit hysteresis so the indoor map does not disappear when the + * user is just near the outer wall rather than truly outside. + * + * @param polygon original polygon vertices + * @param bufferMeters buffer distance in meters to expand outward + * @return a new polygon with each vertex shifted outward + */ + public static List expandPolygon(List polygon, double bufferMeters) { + // Compute centroid + double centLat = 0, centLng = 0; + for (LatLng p : polygon) { + centLat += p.latitude; + centLng += p.longitude; + } + centLat /= polygon.size(); + centLng /= polygon.size(); + + List expanded = new ArrayList<>(); + for (LatLng vertex : polygon) { + double dLat = vertex.latitude - centLat; + double dLng = vertex.longitude - centLng; + + // Convert lat/lng deltas to meters for distance calculation + double dLatM = dLat * 111320.0; + double dLngM = dLng * 111320.0 * Math.cos(Math.toRadians(centLat)); + double dist = Math.sqrt(dLatM * dLatM + dLngM * dLngM); + + if (dist > 0) { + double scale = (dist + bufferMeters) / dist; + expanded.add(new LatLng( + centLat + dLat * scale, + centLng + dLng * scale)); + } else { + expanded.add(vertex); + } + } + return expanded; + } + + /** + * Clamps a point to the nearest edge of the polygon if it is outside. + * If the point is already inside the polygon, it is returned unchanged. + * Uses orthogonal projection onto each edge segment to find the closest + * boundary point. + * + * @param point the point to clamp + * @param polygon the building boundary polygon + * @return the original point if inside, or the nearest boundary point if outside + */ + public static LatLng clampToPolygon(LatLng point, List polygon) { + if (pointInPolygon(point, polygon)) { + return point; + } + + double bestDistSq = Double.MAX_VALUE; + LatLng bestPoint = point; + + for (int i = 0; i < polygon.size(); i++) { + LatLng a = polygon.get(i); + LatLng b = polygon.get((i + 1) % polygon.size()); + + LatLng projected = projectOntoSegment(point, a, b); + double dLat = projected.latitude - point.latitude; + double dLng = projected.longitude - point.longitude; + double distSq = dLat * dLat + dLng * dLng; + + if (distSq < bestDistSq) { + bestDistSq = distSq; + bestPoint = projected; + } + } + return bestPoint; + } + + /** + * Projects a point onto a line segment (a→b), clamping to the segment + * endpoints if the projection falls outside the segment. + * + * @param p the point to project + * @param a segment start + * @param b segment end + * @return the closest point on segment [a, b] to p + */ + private static LatLng projectOntoSegment(LatLng p, LatLng a, LatLng b) { + double abLat = b.latitude - a.latitude; + double abLng = b.longitude - a.longitude; + double apLat = p.latitude - a.latitude; + double apLng = p.longitude - a.longitude; + + double abLenSq = abLat * abLat + abLng * abLng; + if (abLenSq == 0) return a; // degenerate segment + + double t = (apLat * abLat + apLng * abLng) / abLenSq; + t = Math.max(0, Math.min(1, t)); // clamp to [0, 1] + + return new LatLng(a.latitude + t * abLat, a.longitude + t * abLng); + } + + /** + * Function to check if a point is in the Campus area (Nucleus + Library combined) + * @param point the point to be checked + * @return True if point is in either Nucleus or Library area + */ + public static boolean inCampus(LatLng point){ + return pointInPolygon(point, CAMPUS_POLYGON); + } + /** * Function to check if point in polygon (approximates earth to be flat) * Ray casting algorithm https://en.wikipedia.org/wiki/Point_in_polygon diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index f8058603..5de9fe33 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -29,11 +29,18 @@ public class IndoorMapManager { private static final String TAG = "IndoorMapManager"; + // Hysteresis buffer distances (meters) for building boundary detection. + // Entry buffer (negative = shrink polygon): user must be clearly inside before triggering. + // Exit buffer (positive = expand polygon): user must be clearly outside before clearing. + private static final double ENTRY_BUFFER_METERS = -5.0; + private static final double EXIT_BUFFER_METERS = 15.0; + /** Building identifiers for tracking which building the user is in. */ public static final int BUILDING_NONE = 0; public static final int BUILDING_NUCLEUS = 1; public static final int BUILDING_LIBRARY = 2; public static final int BUILDING_MURCHISON = 3; + public static final int BUILDING_CAMPUS = 4; // Nucleus + Library combined private GoogleMap gMap; private LatLng currentLocation; @@ -49,6 +56,18 @@ public class IndoorMapManager { // Per-floor vector shape data for the current building private List currentFloorShapes; + // Campus mode: floor shapes for both buildings loaded simultaneously + private List nucleusFloorShapes; + private List libraryFloorShapes; + + // Cached wall segments for current floor [start, end] LatLng pairs + private List cachedWallSegments; + private int cachedWallFloor = -1; + private int cachedWallBuilding = BUILDING_NONE; + + // Consecutive collision counter — releases position when stuck + private int consecutiveCollisions = 0; + // Average floor heights per building (meters), used for barometric auto-floor public static final float NUCLEUS_FLOOR_HEIGHT = 4.2F; public static final float LIBRARY_FLOOR_HEIGHT = 3.6F; @@ -59,6 +78,23 @@ public class IndoorMapManager { private static final int ROOM_STROKE = Color.argb(180, 33, 150, 243); private static final int ROOM_FILL = Color.argb(40, 33, 150, 243); private static final int DEFAULT_STROKE = Color.argb(150, 100, 100, 100); + private static final int STAIRS_STROKE = Color.argb(200, 76, 175, 80); + private static final int STAIRS_FILL = Color.argb(60, 76, 175, 80); + private static final int LIFT_STROKE = Color.argb(200, 255, 152, 0); + private static final int LIFT_FILL = Color.argb(60, 255, 152, 0); + + // Proximity threshold (meters) for stairs/lift features to allow floor changes + private static final double VERTICAL_TRANSPORT_PROXIMITY_METERS = 10.0; + + // Wall collision constants + private static final String WALL_TAG = "WALL_COLLISION"; + private static final double WALL_METERS_PER_DEG = 111320.0; + private static final double WALL_BUFFER_M = 0.3; // Min distance from wall after slide (meters) + private static final double BOUNCE_DISTANCE_M = 0.5; // Bounce-back distance from wall (meters) + private static final double HEAD_ON_DOT_THRESHOLD = 0.5; // cos(60°): above = bounce, below = slide + private static final double MAX_CONSTRAIN_DIST_M = 2.0; // Skip wall check for large fusion jumps (meters) + private static final int MAX_CONSECUTIVE_BOUNCE = 2; // Switch from bounce to force-slide after N hits + private static final int MAX_CONSECUTIVE_COLLISIONS = 8; // Release position entirely after N hits /** * Constructor to set the map instance. @@ -86,6 +122,12 @@ public void setCurrentLocation(LatLng currentLocation) { * @return the floor height of the current building the user is in */ public float getFloorHeight() { + if (currentBuilding == BUILDING_CAMPUS && currentLocation != null) { + if (BuildingPolygon.inLibrary(currentLocation)) { + return LIBRARY_FLOOR_HEIGHT; + } + return NUCLEUS_FLOOR_HEIGHT; + } return floorHeight; } @@ -117,6 +159,88 @@ public int getCurrentFloor() { return currentFloor; } + /** + * Clamps the given position to within the current building's outer boundary. + * If the user is detected as being inside a building and the position falls + * outside that building's polygon, the position is snapped to the nearest + * point on the building boundary. If the user is not in any building, or the + * position is already inside, the original position is returned unchanged. + * + * @param position the fused position to clamp + * @return the clamped position (unchanged if inside or not in a building) + */ + public LatLng clampToBuildingBoundary(LatLng position) { + if (currentBuilding == BUILDING_NONE || position == null) { + return position; + } + + // Determine the boundary polygon for the current building + List polygon = getCurrentBuildingPolygon(); + if (polygon == null || polygon.size() < 3) { + return position; + } + + // If already inside, no clamping needed + if (BuildingPolygon.pointInPolygon(position, polygon)) { + return position; + } + + // Position is outside — clamp to the nearest boundary edge + LatLng clamped = BuildingPolygon.clampToPolygon(position, polygon); + Log.e(TAG, "WALL_CLAMP: position outside building boundary, clamped" + + " | original=(" + String.format("%.7f", position.latitude) + + "," + String.format("%.7f", position.longitude) + ")" + + " | clamped=(" + String.format("%.7f", clamped.latitude) + + "," + String.format("%.7f", clamped.longitude) + ")" + + " | building=" + currentBuilding); + return clamped; + } + + /** + * Returns the boundary polygon for the current building, preferring + * the real API outline over the legacy hard-coded rectangle. + * + * @return polygon vertices, or null if no building is active + */ + private List getCurrentBuildingPolygon() { + String targetApiName; + List fallbackPolygon; + switch (currentBuilding) { + case BUILDING_NUCLEUS: + targetApiName = "nucleus_building"; + fallbackPolygon = BuildingPolygon.NUCLEUS_POLYGON; + break; + case BUILDING_LIBRARY: + targetApiName = "library"; + fallbackPolygon = BuildingPolygon.LIBRARY_POLYGON; + break; + case BUILDING_MURCHISON: + targetApiName = "murchison_house"; + fallbackPolygon = BuildingPolygon.MURCHISON_POLYGON; + break; + case BUILDING_CAMPUS: + // Campus uses the combined bounding polygon + return BuildingPolygon.CAMPUS_POLYGON; + default: + return null; + } + + // Try API polygon first (more accurate real outline) + List apiBuildings = + SensorFusion.getInstance().getFloorplanBuildings(); + for (FloorplanApiClient.BuildingInfo building : apiBuildings) { + if (targetApiName.equals(building.getName())) { + List outline = building.getOutlinePolygon(); + if (outline != null && outline.size() >= 3) { + return outline; + } + } + } + + // Fallback: legacy hard-coded polygon + return fallbackPolygon; + } + /** * Returns the display name for the current floor (e.g. "LG", "G", "1"). * Falls back to the numeric index if no display name is available. @@ -124,6 +248,10 @@ public int getCurrentFloor() { * @return human-readable floor label */ public String getCurrentFloorDisplayName() { + if (currentBuilding == BUILDING_CAMPUS && nucleusFloorShapes != null + && currentFloor >= 0 && currentFloor < nucleusFloorShapes.size()) { + return nucleusFloorShapes.get(currentFloor).getDisplayName(); + } if (currentFloorShapes != null && currentFloor >= 0 && currentFloor < currentFloorShapes.size()) { @@ -143,13 +271,352 @@ public int getAutoFloorBias() { switch (currentBuilding) { case BUILDING_NUCLEUS: case BUILDING_MURCHISON: - return 1; // LG at index 0, so G = index 1 + case BUILDING_CAMPUS: // Campus uses Nucleus indexing (LG at index 0) + return 1; case BUILDING_LIBRARY: default: return 0; // G at index 0 } } + //region Vertical transport proximity (stairs/lift gate for floor changes) + + /** + * Checks all floors of the current building for "stairs" or "lift" features + * and returns the type of the nearest one within the proximity threshold. + * + * @param position the user's current position + * @return "stairs", "lift", or "unknown" (no map data) if within threshold; null if too far + */ + public String getNearbyVerticalTransport(LatLng position) { + if (position == null) return null; + + // Collect all floor shape sets to scan + List> allShapeSets = new ArrayList<>(); + if (currentBuilding == BUILDING_CAMPUS) { + if (nucleusFloorShapes != null) allShapeSets.add(nucleusFloorShapes); + if (libraryFloorShapes != null) allShapeSets.add(libraryFloorShapes); + } else if (currentFloorShapes != null) { + allShapeSets.add(currentFloorShapes); + } + if (allShapeSets.isEmpty()) return null; + + boolean hasAnyTransport = false; + String nearest = null; + double nearestDist = Double.MAX_VALUE; + + // Scan all floors of all buildings — stairs/lifts span multiple levels + for (List shapeSet : allShapeSets) { + for (FloorplanApiClient.FloorShapes floor : shapeSet) { + for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { + String type = feature.getIndoorType(); + if (!"stairs".equals(type) && !"lift".equals(type)) continue; + + hasAnyTransport = true; + + for (List part : feature.getParts()) { + for (LatLng coord : part) { + double dist = distanceInMeters(position, coord); + if (dist < nearestDist) { + nearestDist = dist; + nearest = type; + } + } + } + } + } + } + + if (!hasAnyTransport) { + Log.e(TAG, "FLOOR_GATE: no stairs/lift features in map data, allowing floor change"); + return "unknown"; + } + + if (nearestDist <= VERTICAL_TRANSPORT_PROXIMITY_METERS) { + Log.e(TAG, "FLOOR_GATE: near " + nearest + + " | dist=" + String.format("%.1f", nearestDist) + "m" + + " | threshold=" + VERTICAL_TRANSPORT_PROXIMITY_METERS + "m"); + return nearest; + } + + Log.e(TAG, "FLOOR_GATE: NOT near any vertical transport" + + " | nearest=" + nearest + + " | dist=" + String.format("%.1f", nearestDist) + "m" + + " | threshold=" + VERTICAL_TRANSPORT_PROXIMITY_METERS + "m" + + " | pos=(" + String.format("%.6f", position.latitude) + + "," + String.format("%.6f", position.longitude) + ")"); + return null; + } + + /** + * Convenience check: is the user near any stairs or lift feature? + * + * @param position the user's current position + * @return true if near stairs or lift (or no map data available) + */ + public boolean isNearStairsOrLift(LatLng position) { + return getNearbyVerticalTransport(position) != null; + } + + /** + * Approximate distance in meters between two LatLng points (flat-earth). + */ + private static double distanceInMeters(LatLng a, LatLng b) { + double dLat = (b.latitude - a.latitude) * 111320.0; + double dLng = (b.longitude - a.longitude) * 111320.0 + * Math.cos(Math.toRadians(a.latitude)); + return Math.sqrt(dLat * dLat + dLng * dLng); + } + + //endregion + + //region Wall collision detection and response + + /** + * Caches wall line segments for the given floor from the floorplan API data. + * Extracts segments from features with indoorType "wall", handling both + * LineString/MultiLineString and Polygon/MultiPolygon geometries. + * + * @param floor the floor index to cache walls for + */ + private void cacheWallSegments(int floor) { + cachedWallSegments = new ArrayList<>(); + cachedWallFloor = floor; + cachedWallBuilding = currentBuilding; + + if (currentBuilding == BUILDING_CAMPUS) { + // Campus mode: collect walls from both buildings + int nucIdx = floor; // Nucleus uses unified campus index directly + if (nucleusFloorShapes != null && nucIdx >= 0 && nucIdx < nucleusFloorShapes.size()) { + addWallSegmentsFromFloor(nucleusFloorShapes.get(nucIdx)); + } + int libIdx = campusToLibraryFloorIndex(floor); + if (libraryFloorShapes != null && libIdx >= 0 && libIdx < libraryFloorShapes.size()) { + addWallSegmentsFromFloor(libraryFloorShapes.get(libIdx)); + } + Log.e(WALL_TAG, "Cached " + cachedWallSegments.size() + " wall segments (campus)" + + " | floor=" + floor + " | nucIdx=" + nucIdx + " | libIdx=" + libIdx); + return; + } + + if (currentFloorShapes == null || floor < 0 || floor >= currentFloorShapes.size()) { + Log.e(WALL_TAG, "Cannot cache walls: no floor data" + + " | floor=" + floor + " | building=" + currentBuilding); + return; + } + + addWallSegmentsFromFloor(currentFloorShapes.get(floor)); + + Log.e(WALL_TAG, "Cached " + cachedWallSegments.size() + " wall segments" + + " | floor=" + floor + + " | floorName=" + currentFloorShapes.get(floor).getDisplayName() + + " | building=" + currentBuilding); + } + + /** + * Extracts wall segments from a single floor's features and adds them to + * {@link #cachedWallSegments}. + */ + private void addWallSegmentsFromFloor(FloorplanApiClient.FloorShapes floorData) { + for (FloorplanApiClient.MapShapeFeature feature : floorData.getFeatures()) { + if (!"wall".equals(feature.getIndoorType())) continue; + + String geoType = feature.getGeometryType(); + boolean isPolygon = "MultiPolygon".equals(geoType) || "Polygon".equals(geoType); + + for (List part : feature.getParts()) { + if (part.size() < 2) continue; + for (int i = 0; i < part.size() - 1; i++) { + cachedWallSegments.add(new LatLng[]{part.get(i), part.get(i + 1)}); + } + if (isPolygon && part.size() >= 3) { + cachedWallSegments.add(new LatLng[]{part.get(part.size() - 1), part.get(0)}); + } + } + } + } + + /** + * Maps a campus unified floor index to the Library's floor index. + * Campus index 1 (Nucleus G) = Library index 0 (Library G). + * Returns -1 if no corresponding Library floor exists. + */ + private int campusToLibraryFloorIndex(int campusFloor) { + int libIdx = campusFloor - 1; // Nucleus LG is index 0, Library G is campus index 1 + if (libIdx < 0 || libraryFloorShapes == null || libIdx >= libraryFloorShapes.size()) { + return -1; + } + return libIdx; + } + + /** + * Constrains movement to respect inner wall boundaries on the current floor. + * Detects if the movement from previousPos to newPos crosses any wall segment: + *
    + *
  • Side/glancing collision (angle > 60° from wall normal): slides along the wall
    • + *
  • Head-on collision (angle <= 60° from wall normal): bounces back from the wall
    • + *
+ * + * @param previousPos the user's previous position (before this step) + * @param newPos the user's new unconstrained position + * @return the constrained position, or newPos unchanged if no wall was crossed + */ + public LatLng constrainToWalls(LatLng previousPos, LatLng newPos) { + if (currentBuilding == BUILDING_NONE || currentFloorShapes == null + || previousPos == null || newPos == null) { + return newPos; + } + + // Refresh cache if floor or building changed + if (cachedWallFloor != currentFloor || cachedWallBuilding != currentBuilding) { + cacheWallSegments(currentFloor); + } + + if (cachedWallSegments == null || cachedWallSegments.isEmpty()) { + return newPos; + } + + // Use previousPos as reference origin for lat/lng -> meter conversion + double refLat = previousPos.latitude; + double cosLat = Math.cos(Math.toRadians(refLat)); + + // Movement vector in meters (previousPos = origin) + double moveE = (newPos.longitude - previousPos.longitude) * WALL_METERS_PER_DEG * cosLat; + double moveN = (newPos.latitude - previousPos.latitude) * WALL_METERS_PER_DEG; + double moveDist = Math.sqrt(moveE * moveE + moveN * moveN); + + if (moveDist < 0.01) return newPos; // No significant movement + + // Skip wall constraint for large fusion jumps (GPS/WiFi corrections) + // — only constrain normal walking steps, not sensor fusion teleports + if (moveDist > MAX_CONSTRAIN_DIST_M) { + Log.e(WALL_TAG, "SKIP: moveDist=" + String.format("%.2f", moveDist) + + "m > " + MAX_CONSTRAIN_DIST_M + "m (fusion correction, not walking)"); + return newPos; + } + + // Find the closest wall intersection along the movement vector + double closestT = Double.MAX_VALUE; + double[] hitWall = null; // [e1, n1, e2, n2] in meters + + for (LatLng[] wall : cachedWallSegments) { + // Convert wall endpoints to meters relative to previousPos + double wE1 = (wall[0].longitude - previousPos.longitude) * WALL_METERS_PER_DEG * cosLat; + double wN1 = (wall[0].latitude - previousPos.latitude) * WALL_METERS_PER_DEG; + double wE2 = (wall[1].longitude - previousPos.longitude) * WALL_METERS_PER_DEG * cosLat; + double wN2 = (wall[1].latitude - previousPos.latitude) * WALL_METERS_PER_DEG; + + double wdE = wE2 - wE1; + double wdN = wN2 - wN1; + + // 2D cross product of movement direction and wall direction + double cross = moveE * wdN - moveN * wdE; + if (Math.abs(cross) < 1e-10) continue; // Parallel — no intersection + + // Line-segment intersection parameters (t for movement, u for wall) + double t = (wE1 * wdN - wN1 * wdE) / cross; + double u = (moveN * wE1 - moveE * wN1) / cross; + + // Valid intersection: t ∈ (0, 1] on movement, u ∈ [0, 1] on wall + if (t > 0.001 && t <= 1.0 && u >= -0.01 && u <= 1.01) { + if (t < closestT) { + closestT = t; + hitWall = new double[]{wE1, wN1, wE2, wN2}; + } + } + } + + if (hitWall == null) { + if (consecutiveCollisions > 0) { + Log.e(WALL_TAG, "CLEAR: no collision, resetting counter (was " + consecutiveCollisions + ")"); + } + consecutiveCollisions = 0; + return newPos; // No wall crossed + } + + consecutiveCollisions++; + + // Two-tier response (no release — never cross walls): + // 1-3: bounce (normal wall response) + // 4+: force slide along wall (try to find a way around) + boolean forceSlide = consecutiveCollisions > MAX_CONSECUTIVE_BOUNCE; + + // Intersection point (meters from previousPos) + double intE = closestT * moveE; + double intN = closestT * moveN; + + // Wall direction vector (normalized) + double wallDE = hitWall[2] - hitWall[0]; + double wallDN = hitWall[3] - hitWall[1]; + double wallLen = Math.sqrt(wallDE * wallDE + wallDN * wallDN); + if (wallLen < 0.001) return newPos; + wallDE /= wallLen; + wallDN /= wallLen; + + // Wall normal (perpendicular to wall), oriented toward user's previous position + double normalE = -wallDN; + double normalN = wallDE; + if (normalE * (-intE) + normalN * (-intN) < 0) { + normalE = -normalE; + normalN = -normalN; + } + + // Movement direction (normalized) + double moveDirE = moveE / moveDist; + double moveDirN = moveN / moveDist; + + // How "head-on" the collision is: 0 = parallel to wall, 1 = perpendicular + double dotMoveNormal = Math.abs(moveDirE * normalE + moveDirN * normalN); + + double resultE, resultN; + + if (!forceSlide && dotMoveNormal > HEAD_ON_DOT_THRESHOLD) { + // HEAD-ON collision: bounce back from wall + resultE = intE + normalE * BOUNCE_DISTANCE_M; + resultN = intN + normalN * BOUNCE_DISTANCE_M; + + Log.e(WALL_TAG, "BOUNCE | angle=" + + String.format("%.1f", Math.toDegrees(Math.acos(Math.min(1.0, dotMoveNormal)))) + + "deg | dot=" + String.format("%.3f", dotMoveNormal) + + " | hit=(" + String.format("%.2f", intE) + "," + String.format("%.2f", intN) + ")m" + + " | result=(" + String.format("%.2f", resultE) + "," + String.format("%.2f", resultN) + ")m" + + " | bounceBack=" + BOUNCE_DISTANCE_M + "m" + + " | moveDist=" + String.format("%.3f", moveDist) + "m"); + } else { + // SLIDE: either glancing angle, or forced slide to prevent stuck-in-wall loop + double remainE = moveE - intE; + double remainN = moveN - intN; + double slideAmount = remainE * wallDE + remainN * wallDN; + + resultE = intE + slideAmount * wallDE + normalE * WALL_BUFFER_M; + resultN = intN + slideAmount * wallDN + normalN * WALL_BUFFER_M; + + Log.e(WALL_TAG, (forceSlide ? "FORCE_SLIDE" : "SLIDE") + " | angle=" + + String.format("%.1f", Math.toDegrees(Math.acos(Math.min(1.0, dotMoveNormal)))) + + "deg | dot=" + String.format("%.3f", dotMoveNormal) + + " | hit=(" + String.format("%.2f", intE) + "," + String.format("%.2f", intN) + ")m" + + " | slide=" + String.format("%.3f", slideAmount) + "m" + + " | result=(" + String.format("%.2f", resultE) + "," + String.format("%.2f", resultN) + ")m" + + " | moveDist=" + String.format("%.3f", moveDist) + "m"); + } + + // Convert result back to LatLng + double resultLat = previousPos.latitude + resultN / WALL_METERS_PER_DEG; + double resultLng = previousPos.longitude + resultE / (WALL_METERS_PER_DEG * cosLat); + + Log.e(WALL_TAG, "Position constrained" + + " | original=(" + String.format("%.7f", newPos.latitude) + + "," + String.format("%.7f", newPos.longitude) + ")" + + " | constrained=(" + String.format("%.7f", resultLat) + + "," + String.format("%.7f", resultLng) + ")" + + " | floor=" + currentFloor + " | building=" + currentBuilding + + " | wallSegments=" + cachedWallSegments.size()); + + return new LatLng(resultLat, resultLng); + } + + //endregion + /** * Sets the floor to display. When called from auto-floor, the floor number * is a logical floor (0=G, -1=LG, 1=Floor 1, etc.) and the building bias @@ -159,19 +626,31 @@ public int getAutoFloorBias() { * @param autoFloor true if called by auto-floor feature */ public void setCurrentFloor(int newFloor, boolean autoFloor) { - if (currentFloorShapes == null || currentFloorShapes.isEmpty()) return; + int maxFloors = getMaxFloorCount(); + if (maxFloors <= 0) return; if (autoFloor) { newFloor += getAutoFloorBias(); } - if (newFloor >= 0 && newFloor < currentFloorShapes.size() + if (newFloor >= 0 && newFloor < maxFloors && newFloor != this.currentFloor) { this.currentFloor = newFloor; drawFloorShapes(newFloor); } } + /** + * Returns the total number of displayable floors. + * In campus mode, uses the Nucleus floor count (the building with more floors). + */ + private int getMaxFloorCount() { + if (currentBuilding == BUILDING_CAMPUS) { + return (nucleusFloorShapes != null) ? nucleusFloorShapes.size() : 0; + } + return (currentFloorShapes != null) ? currentFloorShapes.size() : 0; + } + /** * Increments the current floor and changes to a higher floor's map * (if a higher floor exists). @@ -198,51 +677,74 @@ public void decreaseFloor() { */ private void setBuildingOverlay() { try { - int detected = detectCurrentBuilding(); + // When outside, use shrunk boundary for entry (harder to falsely enter); + // when inside, use normal boundary for the initial check. + double entryBuffer = isIndoorMapSet ? 0.0 : ENTRY_BUFFER_METERS; + int detected = detectCurrentBuilding(entryBuffer); + + // Promote Nucleus or Library to Campus so both maps load together + if (detected == BUILDING_NUCLEUS || detected == BUILDING_LIBRARY) { + detected = BUILDING_CAMPUS; + } + boolean inAnyBuilding = (detected != BUILDING_NONE); if (inAnyBuilding && !isIndoorMapSet) { currentBuilding = detected; - String apiName; - - switch (detected) { - case BUILDING_NUCLEUS: - apiName = "nucleus_building"; - currentFloor = 1; - floorHeight = NUCLEUS_FLOOR_HEIGHT; - break; - case BUILDING_LIBRARY: - apiName = "library"; - currentFloor = 0; - floorHeight = LIBRARY_FLOOR_HEIGHT; - break; - case BUILDING_MURCHISON: - apiName = "murchison_house"; - currentFloor = 1; - floorHeight = MURCHISON_FLOOR_HEIGHT; - break; - default: - return; - } - // Load floor shapes from cached API data - FloorplanApiClient.BuildingInfo building = - SensorFusion.getInstance().getFloorplanBuilding(apiName); - if (building != null) { - currentFloorShapes = building.getFloorShapesList(); - } - - if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { - drawFloorShapes(currentFloor); - isIndoorMapSet = true; + if (detected == BUILDING_CAMPUS) { + // Load BOTH buildings' floor shapes + FloorplanApiClient.BuildingInfo nucleusInfo = + SensorFusion.getInstance().getFloorplanBuilding("nucleus_building"); + FloorplanApiClient.BuildingInfo libraryInfo = + SensorFusion.getInstance().getFloorplanBuilding("library"); + + nucleusFloorShapes = (nucleusInfo != null) + ? nucleusInfo.getFloorShapesList() : null; + libraryFloorShapes = (libraryInfo != null) + ? libraryInfo.getFloorShapesList() : null; + + // Primary floor shapes = Nucleus (more floors) + currentFloorShapes = nucleusFloorShapes; + currentFloor = 1; // Ground floor in Nucleus indexing (LG=0, G=1) + floorHeight = NUCLEUS_FLOOR_HEIGHT; + + if (getMaxFloorCount() > 0) { + drawFloorShapes(currentFloor); + isIndoorMapSet = true; + Log.e(TAG, "Campus mode activated: Nucleus floors=" + + (nucleusFloorShapes != null ? nucleusFloorShapes.size() : 0) + + " Library floors=" + + (libraryFloorShapes != null ? libraryFloorShapes.size() : 0)); + } + } else { + // Murchison (unchanged) + currentFloor = 1; + floorHeight = MURCHISON_FLOOR_HEIGHT; + + FloorplanApiClient.BuildingInfo building = + SensorFusion.getInstance().getFloorplanBuilding("murchison_house"); + if (building != null) { + currentFloorShapes = building.getFloorShapesList(); + } + + if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { + drawFloorShapes(currentFloor); + isIndoorMapSet = true; + } } } else if (!inAnyBuilding && isIndoorMapSet) { - clearDrawnShapes(); - isIndoorMapSet = false; - currentBuilding = BUILDING_NONE; - currentFloor = 0; - currentFloorShapes = null; + // Use expanded boundary for exit — only clear when truly outside + if (!isInsideCurrentBuildingWithBuffer()) { + clearDrawnShapes(); + isIndoorMapSet = false; + currentBuilding = BUILDING_NONE; + currentFloor = 0; + currentFloorShapes = null; + nucleusFloorShapes = null; + libraryFloorShapes = null; + } } } catch (Exception ex) { Log.e(TAG, "Error with overlay: " + ex.toString()); @@ -258,10 +760,30 @@ private void setBuildingOverlay() { private void drawFloorShapes(int floorIndex) { clearDrawnShapes(); + if (currentBuilding == BUILDING_CAMPUS) { + // Campus mode: draw shapes from both Nucleus and Library + if (nucleusFloorShapes != null && floorIndex >= 0 + && floorIndex < nucleusFloorShapes.size()) { + drawSingleFloorFeatures(nucleusFloorShapes.get(floorIndex)); + } + int libIdx = campusToLibraryFloorIndex(floorIndex); + if (libraryFloorShapes != null && libIdx >= 0 + && libIdx < libraryFloorShapes.size()) { + drawSingleFloorFeatures(libraryFloorShapes.get(libIdx)); + } + return; + } + if (currentFloorShapes == null || floorIndex < 0 || floorIndex >= currentFloorShapes.size()) return; - FloorplanApiClient.FloorShapes floor = currentFloorShapes.get(floorIndex); + drawSingleFloorFeatures(currentFloorShapes.get(floorIndex)); + } + + /** + * Draws all features of a single floor onto the map (without clearing first). + */ + private void drawSingleFloorFeatures(FloorplanApiClient.FloorShapes floor) { for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { String geoType = feature.getGeometryType(); String indoorType = feature.getIndoorType(); @@ -309,6 +831,8 @@ private void clearDrawnShapes() { private int getStrokeColor(String indoorType) { if ("wall".equals(indoorType)) return WALL_STROKE; if ("room".equals(indoorType)) return ROOM_STROKE; + if ("stairs".equals(indoorType)) return STAIRS_STROKE; + if ("lift".equals(indoorType)) return LIFT_STROKE; return DEFAULT_STROKE; } @@ -320,37 +844,55 @@ private int getStrokeColor(String indoorType) { */ private int getFillColor(String indoorType) { if ("room".equals(indoorType)) return ROOM_FILL; + if ("stairs".equals(indoorType)) return STAIRS_FILL; + if ("lift".equals(indoorType)) return LIFT_FILL; return Color.TRANSPARENT; } /** - * Detects which building the user is currently in. - * Checks floorplan API outline polygons first; falls back to legacy - * hard-coded rectangular boundaries if no API match is found. + * Detects which building the user is currently in, optionally applying a + * buffer to the boundary polygons. A negative buffer shrinks the polygon + * (for stricter entry detection); zero uses the original boundary. * + * @param bufferMeters buffer in meters (negative = shrink, 0 = original) * @return building type constant, or {@link #BUILDING_NONE} */ - private int detectCurrentBuilding() { + private int detectCurrentBuilding(double bufferMeters) { // Phase 1: API real polygon outlines List apiBuildings = SensorFusion.getInstance().getFloorplanBuildings(); for (FloorplanApiClient.BuildingInfo building : apiBuildings) { List outline = building.getOutlinePolygon(); - if (outline != null && outline.size() >= 3 - && BuildingPolygon.pointInPolygon(currentLocation, outline)) { - int type = resolveBuildingType(building.getName()); - if (type != BUILDING_NONE) return type; + if (outline != null && outline.size() >= 3) { + List poly = (bufferMeters != 0.0) + ? BuildingPolygon.expandPolygon(outline, bufferMeters) + : outline; + if (BuildingPolygon.pointInPolygon(currentLocation, poly)) { + int type = resolveBuildingType(building.getName()); + if (type != BUILDING_NONE) return type; + } } } // Phase 2: legacy hard-coded fallback - if (BuildingPolygon.inNucleus(currentLocation)) return BUILDING_NUCLEUS; - if (BuildingPolygon.inLibrary(currentLocation)) return BUILDING_LIBRARY; - if (BuildingPolygon.inMurchison(currentLocation)) return BUILDING_MURCHISON; + if (checkLegacyBuilding(BuildingPolygon.NUCLEUS_POLYGON, bufferMeters)) + return BUILDING_NUCLEUS; + if (checkLegacyBuilding(BuildingPolygon.LIBRARY_POLYGON, bufferMeters)) + return BUILDING_LIBRARY; + if (checkLegacyBuilding(BuildingPolygon.MURCHISON_POLYGON, bufferMeters)) + return BUILDING_MURCHISON; return BUILDING_NONE; } + /** Checks if currentLocation is inside a legacy polygon with optional buffer. */ + private boolean checkLegacyBuilding(List polygon, double bufferMeters) { + List poly = (bufferMeters != 0.0) + ? BuildingPolygon.expandPolygon(polygon, bufferMeters) + : polygon; + return BuildingPolygon.pointInPolygon(currentLocation, poly); + } + /** * Maps a floorplan API building name to a building type constant. * @@ -367,6 +909,61 @@ private int resolveBuildingType(String apiName) { } } + /** + * Checks whether the current location is inside the current building's + * boundary expanded by {@link #EXIT_BUFFER_METERS}. Used for exit + * hysteresis so the floor plan stays visible when the user is near the + * outer wall rather than clearly outside. + * + * @return true if inside the expanded boundary of the current building + */ + private boolean isInsideCurrentBuildingWithBuffer() { + if (currentBuilding == BUILDING_CAMPUS) { + // Campus exit: must leave the expanded campus bounding rectangle + List expanded = + BuildingPolygon.expandPolygon(BuildingPolygon.CAMPUS_POLYGON, EXIT_BUFFER_METERS); + return BuildingPolygon.pointInPolygon(currentLocation, expanded); + } + + String targetApiName; + List fallbackPolygon; + switch (currentBuilding) { + case BUILDING_NUCLEUS: + targetApiName = "nucleus_building"; + fallbackPolygon = BuildingPolygon.NUCLEUS_POLYGON; + break; + case BUILDING_LIBRARY: + targetApiName = "library"; + fallbackPolygon = BuildingPolygon.LIBRARY_POLYGON; + break; + case BUILDING_MURCHISON: + targetApiName = "murchison_house"; + fallbackPolygon = BuildingPolygon.MURCHISON_POLYGON; + break; + default: + return false; + } + + // Try API polygon with buffer first + List apiBuildings = + SensorFusion.getInstance().getFloorplanBuildings(); + for (FloorplanApiClient.BuildingInfo building : apiBuildings) { + if (targetApiName.equals(building.getName())) { + List outline = building.getOutlinePolygon(); + if (outline != null && outline.size() >= 3) { + List expanded = + BuildingPolygon.expandPolygon(outline, EXIT_BUFFER_METERS); + return BuildingPolygon.pointInPolygon(currentLocation, expanded); + } + } + } + + // Fallback: legacy hard-coded polygon with buffer + List expanded = + BuildingPolygon.expandPolygon(fallbackPolygon, EXIT_BUFFER_METERS); + return BuildingPolygon.pointInPolygon(currentLocation, expanded); + } + /** * Draws green polyline indicators around all buildings with available * indoor floor maps. Uses floorplan API outlines when available, diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java index 9765b044..56dc3855 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -8,6 +8,8 @@ import com.openpositioning.PositionMe.sensors.SensorFusion; +import android.util.Log; + import java.util.Arrays; import java.util.Collections; import java.util.List; @@ -141,32 +143,30 @@ public PdrProcessing(Context context) { */ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertime, float headingRad) { if (accelMagnitudeOvertime == null || accelMagnitudeOvertime.size() < MIN_REQUIRED_SAMPLES) { - return new float[]{this.positionX, this.positionY}; // Return current position without update - // - TODO - temporary solution of the empty list issue + Log.e("PDR_DEBUG", "Skipped: accelSamples=" + + (accelMagnitudeOvertime == null ? "null" : accelMagnitudeOvertime.size()) + + " < " + MIN_REQUIRED_SAMPLES); + return new float[]{this.positionX, this.positionY}; } - // Change angle so zero rad is east + // Change angle so zero rad is east (north-clockwise -> east-counterclockwise) float adaptedHeading = (float) (Math.PI/2 - headingRad); // check if accelMagnitudeOvertime is empty if (accelMagnitudeOvertime == null || accelMagnitudeOvertime.isEmpty()) { - // return current position, do not update return new float[]{this.positionX, this.positionY}; } - + // Calculate step length if(!useManualStep) { - //ArrayList accelMagnitudeFiltered = filter(accelMagnitudeOvertime); - // Estimate stride this.stepLength = weibergMinMax(accelMagnitudeOvertime); - // System.err.println("Step Length" + stepLength); } // Increment aggregate variables sumStepLength += stepLength; stepCount++; - // Translate to cartesian coordinate system + // Translate to cartesian coordinate system (x=east, y=north) float x = (float) (stepLength * Math.cos(adaptedHeading)); float y = (float) (stepLength * Math.sin(adaptedHeading)); @@ -174,7 +174,15 @@ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertim this.positionX += x; this.positionY += y; - // return current position + // Detailed heading and step log + Log.e("PDR_DEBUG", "Step #" + stepCount + + " | rawHeading=" + String.format("%.1f", Math.toDegrees(headingRad)) + "deg" + + " | adaptedHeading=" + String.format("%.1f", Math.toDegrees(adaptedHeading)) + "deg" + + " | stepLen=" + String.format("%.3f", stepLength) + "m" + + " | dE=" + String.format("%.3f", x) + " dN=" + String.format("%.3f", y) + + " | posE=" + String.format("%.2f", positionX) + " posN=" + String.format("%.2f", positionY) + + " | accelSamples=" + accelMagnitudeOvertime.size()); + return new float[]{this.positionX, this.positionY}; } diff --git a/app/src/main/res/layout/fragment_recording.xml b/app/src/main/res/layout/fragment_recording.xml index 518e8b75..360017a1 100644 --- a/app/src/main/res/layout/fragment_recording.xml +++ b/app/src/main/res/layout/fragment_recording.xml @@ -81,6 +81,18 @@ app:layout_constraintTop_toBottomOf="@id/currentPositionCard" app:layout_constraintBottom_toTopOf="@id/controlLayout" /> + + + + + + + + + + + + + + + + + diff --git a/app/src/main/res/values/strings.xml b/app/src/main/res/values/strings.xml index 125d27c3..8abd2b67 100644 --- a/app/src/main/res/values/strings.xml +++ b/app/src/main/res/values/strings.xml @@ -114,10 +114,12 @@ Floor height in meters Color 🛰 Show GNSS + 📶 Show WiFi Floor Down button Floor Up button Choose Map ❇️ Auto Floor + 🔄 Smooth GNSS error: Satellite Normal @@ -141,6 +143,9 @@ Exit Indoor Positioning Entering Indoor Positioning mode + Refresh Position + Position refreshed + No GNSS/WiFi position available Trajectory Quality Issues