import 'dart:async';
import 'dart:math' as math;
import 'package:flutter_riverpod/flutter_riverpod.dart';
import 'package:flutter_rotation_sensor/flutter_rotation_sensor.dart';
import '../models/arena_models.dart';
class MotionState {
final double currentYaw;
final double currentPitch;
final String? lockedTargetId;
final double rawYaw;
final double calibrationOffset;
const MotionState({
required this.currentYaw,
required this.currentPitch,
this.lockedTargetId,
required this.rawYaw,
required this.calibrationOffset,
});
MotionState copyWith({
double? currentYaw,
double? currentPitch,
String? lockedTargetId,
double? rawYaw,
double? calibrationOffset,
}) {
return MotionState(
currentYaw: currentYaw ?? this.currentYaw,
currentPitch: currentPitch ?? this.currentPitch,
lockedTargetId: lockedTargetId ?? this.lockedTargetId,
rawYaw: rawYaw ?? this.rawYaw,
calibrationOffset: calibrationOffset ?? this.calibrationOffset,
);
}
}
class UserMotionNotifier extends StateNotifier<MotionState> {
final List<PeerNode> activeNodes;
StreamSubscription? _orientationSub;
double _filteredYaw = 0.0;
double _filteredPitch = 90.0;
double _calibrationOffset = 0.0;
final List<double> _yawBuffer = [];
final List<double> _pitchBuffer = [];
static const int _bufferSize = 15;
DateTime? _lastGyroTime;
bool _isFirstReading = true;
double _lastW = 1.0;
double _lastX = 0.0;
double _lastY = 0.0;
double _lastZ = 0.0;
UserMotionNotifier(this.activeNodes)
: super(const MotionState(
currentYaw: 0.0,
currentPitch: 90.0,
rawYaw: 0.0,
calibrationOffset: 0.0,
));
void startTracking() {
RotationSensor.samplingPeriod = SensorInterval.fastestInterval;
RotationSensor.referenceFrame = ReferenceFrame.arbitrary;
RotationSensor.coordinateSystem = CoordinateSystem.transformed(Axis3.X, Axis3.Y);
_orientationSub = RotationSensor.orientationStream.listen((event) {
final q = event.quaternion;
double targetW = q.w;
double targetX = -q.x;
double targetY = q.y;
double targetZ = -q.z;
if (targetW < 0.0) {
targetW = -targetW;
targetX = -targetX;
targetY = -targetY;
targetZ = -targetZ;
}
double sinyCosp = 2.0 * (targetW * targetZ + targetX * targetY);
double cosyCosp = 1.0 - 2.0 * (targetY * targetY + targetZ * targetZ);
double rawAzimuthRad = math.atan2(sinyCosp, cosyCosp);
double rawAzimuthDeg = rawAzimuthRad * 180.0 / math.pi;
if (rawAzimuthDeg < 0) rawAzimuthDeg += 360.0;
double sinp = 2.0 * (targetW * targetY - targetZ * targetX);
double rawPitchRad = sinp.abs() >= 1.0
? (sinp.sign * math.pi / 2.0)
: math.asin(sinp);
double rawPitchDeg = 90.0 + (rawPitchRad * 180.0 / math.pi);
rawPitchDeg = rawPitchDeg.clamp(90.0, 180.0);
if (_isFirstReading) {
_calibrationOffset = rawAzimuthDeg;
_filteredYaw = 0.0;
_filteredPitch = rawPitchDeg;
_isFirstReading = false;
}
_updateFilters(rawAzimuthDeg, rawPitchDeg);
});
}
void _updateFilters(double newYaw, double newPitch) {
double yawDiff = newYaw - _filteredYaw;
if (yawDiff > 180) newYaw -= 360;
if (yawDiff < -180) newYaw += 360;
_filteredYaw = (0.95 * _filteredYaw + 0.05 * newYaw) % 360.0;
if (_filteredYaw < 0) _filteredYaw += 360.0;
_filteredPitch = 0.95 * _filteredPitch + 0.05 * newPitch;
_yawBuffer.add(_filteredYaw);
_pitchBuffer.add(_filteredPitch);
if (_yawBuffer.length > _bufferSize) _yawBuffer.removeAt(0);
if (_pitchBuffer.length > _bufferSize) _pitchBuffer.removeAt(0);
double smoothedYaw = _yawBuffer.reduce((a, b) => a + b) / _yawBuffer.length;
double smoothedPitch = _pitchBuffer.reduce((a, b) => a + b) / _pitchBuffer.length;
double finalYaw = (smoothedYaw - _calibrationOffset) % 360.0;
if (finalYaw < 0) finalYaw += 360.0;
String? lockedId;
for (var node in activeNodes) {
double targetAngleDeg = node.azimuthAngleRad * 180.0 / math.pi;
double angleDiff = (finalYaw - targetAngleDeg).abs();
double normDiff = angleDiff % 360.0;
if (normDiff > 180.0) {
normDiff = 360.0 - normDiff;
}
double distance = node.scaledDistanceMeters;
double dynamicTolerance = math.max(2.5, math.atan(0.6 / distance) * 180.0 / math.pi);
if (normDiff <= dynamicTolerance) {
lockedId = node.id;
break;
}
}
state = MotionState(
currentYaw: finalYaw,
currentPitch: smoothedPitch,
lockedTargetId: lockedId,
rawYaw: _filteredYaw,
calibrationOffset: _calibrationOffset,
);
}
void recalibrate(double targetAngleDegrees) {
_calibrationOffset = (_filteredYaw - targetAngleDegrees) % 360.0;
if (_calibrationOffset < 0) _calibrationOffset += 360.0;
}
void autoAlignOnHit(String targetId) {
final node = activeNodes.firstWhere((n) => n.id == targetId);
double targetAngleDeg = node.azimuthAngleRad * 180.0 / math.pi;
recalibrate(targetAngleDeg);
}
void dispose() {
_orientationSub?.cancel();
super.dispose();
}
}
final motionProvider = StateNotifierProvider.family<UserMotionNotifier, MotionState, List<PeerNode>>((ref, nodes) {
return UserMotionNotifier(nodes);
});
翻译说明:上述 Dart 代码已翻译为简体中文。
评论 (0)