/// <summary>
/// Change orientation of bubble surface or tube
/// </summary>
/// <param name="e">Event arguments</param>
private void ChangeLevelOrientation(DeviceOrientationChangedEventArgs e)
{
_bubbleSpeed = new Simple3DVector();
if (DeviceOrientationHelper.IsFlat(e.CurrentOrientation))
{
SurfaceShown = true;
DeviceOrientationInfo priorDoi = DeviceOrientationHelper.GetDeviceOrientationInfo(e.PreviousOrientation);
_bubblePosition = new Simple3DVector(priorDoi.NormalGravityVector.X, priorDoi.NormalGravityVector.Y, 0);
}
else
{
DeviceOrientationInfo doi = DeviceOrientationHelper.GetDeviceOrientationInfo(e.CurrentOrientation);
if (DeviceOrientationHelper.IsFlat(e.PreviousOrientation) || (e.PreviousOrientation == DeviceOrientation.Unknown))
{
MoveLevel.Angle = doi.AngleOnXYPlan;
_bubblePosition = new Simple3DVector();
SurfaceShown = false;
}
double accumulatedLoops = MoveLevel.Angle / 360;
accumulatedLoops = Math.Floor(accumulatedLoops + 0.5);
_tubeRotationAnimation.To = 360 * accumulatedLoops + doi.AngleOnXYPlan;
if ((_tubeRotationAnimation.To - MoveLevel.Angle) > 180)
{
_tubeRotationAnimation.To -= 360;
}
if ((_tubeRotationAnimation.To - MoveLevel.Angle) < -180)
{
_tubeRotationAnimation.To += 360;
}
_tubeRotationStoryboard.Begin();
_bubbleDirection = doi.HorizontalAxisPolarity;
}
_levelOrientation = e.CurrentOrientation;
}
/// <summary>
/// Process still signal: calculate average still vector
/// </summary>
private void ProcessStillSignal()
{
Simple3DVector sumVector = new Simple3DVector(0, 0, 0);
int count = 0;
// going over vectors in still signal
// still signal was saved backwards, i.e. newest vectors are first
foreach (Simple3DVector currentStillVector in _stillSignal)
{
// make sure current vector is very still
bool isStillMagnitude = (Math.Abs(_lastStillVector.Magnitude - currentStillVector.Magnitude) < StillMagnitudeWithoutGravitationThreshold);
if (isStillMagnitude)
{
// sum x,y,z values
sumVector += currentStillVector;
++count;
// 20 samples are sufficent
if (count >= MaximumStillVectorsNeededForAverage)
{
break;
}
}
}
// need at least a few vectors to get a good average
if (count >= MinimumStillVectorsNeededForAverage)
{
// calculate average of still vectors
_lastStillVector = sumVector / count;
}
}
private void AddVectorToStillSignal(Simple3DVector currentVector)
{
// add current vector to still signal, newest vectors are first
_stillSignal.AddFirst(currentVector);
// if still signal is getting too big, remove old items
if (_stillSignal.Count > 2 * MaximumStillVectorsNeededForAverage)
{
_stillSignal.RemoveLast();
}
}
/// <summary>
/// Add a vector the shake signal and does some preprocessing
/// </summary>
private void AddVectorToShakeSignal(Simple3DVector currentVector)
{
// remove still vector from current vector
Simple3DVector currentVectorWithoutGravitation = currentVector - _lastStillVector;
// add current vector to shake signal
_shakeSignal.Add(currentVectorWithoutGravitation);
// skip weak vectors
if (currentVectorWithoutGravitation.Magnitude < WeakMagnitudeWithoutGravitationThreshold)
{
return;
}
// classify vector
ShakeType vectorShakeType = ClassifyVectorShakeType(currentVectorWithoutGravitation);
// count vector to histogram
_shakeHistogram[(int)vectorShakeType]++;
}
/// <summary>
/// Classify vector shake type
/// </summary>
private ShakeType ClassifyVectorShakeType(Simple3DVector v)
{
double absX = Math.Abs(v.X);
double absY = Math.Abs(v.Y);
double absZ = Math.Abs(v.Z);
// check if X is the most significant component
if ((absX >= absY) & (absX >= absZ))
{
return(ShakeType.X);
}
// check if Y is the most significant component
if ((absY >= absX) & (absY >= absZ))
{
return(ShakeType.Y);
}
// Z is the most significant component
return(ShakeType.Z);
}
/// <summary>
/// Called when the accelerometer provides a new value
/// </summary>
private void OnAccelerometerHelperReadingChanged(object sender, AccelerometerHelperReadingEventArgs e)
{
// work with optimal vector (without noise)
Simple3DVector currentVector = e.OptimalyFilteredAcceleration;
// check if this vector is considered a shake vector
bool isShakeMagnitude = (Math.Abs(_lastStillVector.Magnitude - currentVector.Magnitude) > ShakeMagnitudeWithoutGravitationThreshold);
// following is a state machine for detection of shake signal start and end
// if still --> shake
if ((!_isInShakeState) && (isShakeMagnitude))
{
// set shake state flag
_isInShakeState = true;
// clear old shake signal
ClearShakeSignal();
// process still signal
ProcessStillSignal();
// add vector to shake signal
AddVectorToShakeSignal(currentVector);
}
// if still --> still
else if ((!_isInShakeState) && (!isShakeMagnitude))
{
// add vector to still signal
AddVectorToStillSignal(currentVector);
}
// if shake --> shake
else if ((_isInShakeState) && (isShakeMagnitude))
{
// add vector to shake signal
AddVectorToShakeSignal(currentVector);
// reset still counter
_stillCounter = 0;
// try to process shake signal
if (ProcessShakeSignal())
{
// shake signal generated, clear used data
ClearShakeSignal();
}
}
// if shake --> still
else if ((_isInShakeState) && (!isShakeMagnitude))
{
// add vector to shake signal
AddVectorToShakeSignal(currentVector);
// count still vectors
_stillCounter++;
// if too much still samples
if (_stillCounter > StillCounterThreshold)
{
// clear old still signal
_stillSignal.Clear();
// add still vectors from shake signal to still signal
for (int i = 0; i < StillCounterThreshold; ++i)
{
// calculate current index element
int currentSampleIndex = _shakeSignal.Count - StillCounterThreshold + i;
// add vector to still signal
AddVectorToStillSignal(currentVector);
}
// remove last samples from shake signal
_shakeSignal.RemoveRange(_shakeSignal.Count - StillCounterThreshold, StillCounterThreshold);
// reset shake state flag
_isInShakeState = false;
_stillCounter = 0;
// try to process shake signal
if (ProcessShakeSignal())
{
ClearShakeSignal();
}
}
}
}
/// <summary>
/// Update the tube level visuals
/// </summary>
private void UpdateTubeBubble(AccelerometerHelperReadingEventArgs e)
{
// ANGLE TEXT
// ----------
// Use filtered accelemeter data (st5ady)
double x = e.OptimallyFilteredAcceleration.X;
double y = e.OptimallyFilteredAcceleration.Y;
double horizontalAcceleration;
double verticalAcceleration;
// Choose appropriate axis based on orientation of level
horizontalAcceleration = _bubbleDirection * (DeviceOrientationHelper.IsPortrait(_levelOrientation) ? x : y);
verticalAcceleration = _bubbleDirection * (DeviceOrientationHelper.IsPortrait(_levelOrientation) ? y : -x); // rotate XY plan by -90
// Convert acceleration vector coordinates to Angles and Magnitude
// Update reading on screen of instant inclination assuming steady device (gravity = measured acceleration)
double magnitudeXYZ = e.OptimallyFilteredAcceleration.Magnitude;
double angle = 0.0;
if (magnitudeXYZ != 0.0)
{
angle = Math.Asin(horizontalAcceleration / magnitudeXYZ) * 180.0 / Math.PI;
}
_angle = angle;
// Display angles as if they were buoyancy force instead of gravity (opposite) since it is
// more natural to match targeted bubble location
_angleText = String.Format("{0:0.0}°", _angle);
// BUBBLE POSITION
// ---------------
// ----------------------------------------------------------
// For simplicity we are approximating that the bubble experiences a lateral attraction force
// proportional to the distance to its target location (top of the glass based on inclination).
// We will neglect the vertical speed of the bubble since the radius of the glass curve is much greater
// than the radius of radius of usable glass surface
// Assume tube curve has a 1m radius
// Destination position is x and y
// Current position is _bubblePosition.X (use only one dimension)
// Update Buoyancy
double lateralAcceleration = (horizontalAcceleration - _bubblePosition.X) * BuoyancyCoef * StandardGravityInMetric;
// Update drag:
double drag = _bubbleSpeed.X * (-ViscosityCoef);
// Update speed:
lateralAcceleration += drag;
lateralAcceleration /= AccelerometerRefreshRate; // impulse
_bubbleSpeed += new Simple3DVector(lateralAcceleration, 0, 0);
// Update position
_bubblePosition += (_bubbleSpeed / AccelerometerRefreshRate);
double edgeRadius = Math.Sin(EdgeGlassAngle);
// Get resulting direction and magnitude of bubble position given X
double magnitudeFlat = Math.Abs(_bubblePosition.X);
double direction = Math.Sign(_bubblePosition.X);
bool atEdge = false;
if (magnitudeFlat > edgeRadius)
{ // Bubble reaches the edge
magnitudeFlat = edgeRadius;
// lossy bouncing when reaching edges
// change direction
_bubbleSpeed *= -EdgeBouncingLossCoef;
// limit bubble position to edge
_bubblePosition = new Simple3DVector(magnitudeFlat * direction, 0, 0);
atEdge = true;
}
// Calculate position of bubble
MoveBubble.X = (_bubblePosition.X / edgeRadius) * ((UsableLateralAmplitude / 2) - (BubbleCanvas.Width / 4));
// Change bubble shape
double stretchRatio;
if (atEdge)
{
TubeBubbleSkew.AngleX = 0;
stretchRatio = 1.0 + (Math.Abs(Math.Sin(angle / 180.0 * Math.PI)) - Math.Sin(EdgeGlassAngle)) * AngleBasedTubeBubbleStrechingAtEdgeCoef;
TubeBubbleScale.ScaleX = 1.0 / stretchRatio;
TubeBubbleScale.ScaleY = SideWayBubbleAtEdgeBoostOnYAxis * stretchRatio;
}
else
{
double horizontalSpeed = Math.Abs(_bubbleSpeed.X);
double horizontalDirection = Math.Sign(_bubbleSpeed.X);
// Stretch is proportional to horizontal speed
stretchRatio = Math.Min(horizontalSpeed * SpeedBubbleStrechingCoef, MaximumBubbleXYStretchRatio - 1.0) + 1.0;
if (stretchRatio < MinimumStretchRatio)
{
stretchRatio = MinimumStretchRatio;
}
TubeBubbleSkew.AngleX = MaximumTubeBubbleSkew * (stretchRatio - 1.0) / (MaximumBubbleXYStretchRatio - 1.0) * horizontalDirection;
// Stretch is also proportional to buoyancy
stretchRatio -= TubeBubbleStretchBuoyancyCoef * ((verticalAcceleration / magnitudeXYZ) - MedianVerticalAcceleration);
if (stretchRatio < MinimumStretchRatio)
{
stretchRatio = MinimumStretchRatio;
}
TubeBubbleScale.ScaleX = 1.0 / stretchRatio;
TubeBubbleScale.ScaleY = MaximumBubbleXYStretchRatio * stretchRatio;
}
}
/// <summary>
/// Update the surface level visuals
/// </summary>
private void UpdateSurfaceBubble(AccelerometerHelperReadingEventArgs e)
{
// ANGLE TEXT
// ----------
// Use filtered accelemeter data (steady)
double x = -e.OptimallyFilteredAcceleration.X; // Orientation compensation
double y = e.OptimallyFilteredAcceleration.Y;
// Convert acceleration vector coordinates to Angles and Magnitude
// Update reading on screen of instant inclination assuming steady device (gravity = measured acceleration)
double magnitudeXYZ = e.OptimallyFilteredAcceleration.Magnitude;
double xAngle = 0.0;
double yAngle = 0.0;
if (magnitudeXYZ != 0.0)
{
xAngle = Math.Asin(x / magnitudeXYZ) * 180.0 / Math.PI;
yAngle = Math.Asin(y / magnitudeXYZ) * 180.0 / Math.PI;
}
_angle = Math.Abs(xAngle) + Math.Abs(yAngle);
// Display angles as if they were buoyancy force instead of gravity (opposite) since it is
// more natural to match targeted bubble location
// Also orientation of Y-axis is opposite of screen for natural upward orientation
_angleText = String.Format("{0:0.0}° {1:0.0}°", xAngle, -yAngle);
// BUBBLE POSITION
// ---------------
// ----------------------------------------------------------
// For simplicity we are approximating that the bubble experiences a lateral attraction force
// proportional to the distance to its target location (top of the glass based on inclination).
// We will neglect the vertical speed of the bubble since the radius of the glass curve is much greater
// than the radius of radius of usable glass surface
// Assume sphere has a 1m radius
// Destination position is x and y
// Current position is _bubblePosition
// Update Buoyancy
Simple3DVector lateralAcceleration = (new Simple3DVector(x, y, 0) - _bubblePosition) * BuoyancyCoef * StandardGravityInMetric;
// Update drag:
Simple3DVector drag = _bubbleSpeed * (-ViscosityCoef);
// Update speed:
lateralAcceleration += drag;
lateralAcceleration /= AccelerometerRefreshRate; // impulse
_bubbleSpeed += lateralAcceleration;
// Update position
_bubblePosition += _bubbleSpeed / AccelerometerRefreshRate;
double edgeRadius = Math.Sin(EdgeGlassAngle);
x = _bubblePosition.X;
y = _bubblePosition.Y;
// Get resulting angle and magnitude of bubble position given X and Y
double angleFlat = Math.Atan2(y, x);
double magnitudeFlat = Math.Sqrt(x * x + y * y);
bool atEdge = false;
if (magnitudeFlat > edgeRadius)
{ // Bubble reaches the edge
magnitudeFlat = edgeRadius;
// lossy bouncing when reaching edges
_bubbleSpeed *= EdgeBouncingLossCoef;
// Mirror movement along center to edge line
_bubbleSpeed = new Simple3DVector(_bubbleSpeed.X * Math.Cos(2 * angleFlat) + _bubbleSpeed.Y * Math.Sin(2 * angleFlat),
_bubbleSpeed.X * Math.Sin(2 * angleFlat) - _bubbleSpeed.Y * Math.Cos(2 * angleFlat),
0);
// change direction on x and y
_bubbleSpeed *= new Simple3DVector(-1, -1, 1);
// limit bubble position to edge
_bubblePosition = new Simple3DVector(magnitudeFlat * Math.Cos(angleFlat), magnitudeFlat * Math.Sin(angleFlat), 0);
atEdge = true;
}
// Set x and y location of the surface level bubble based on angle and magnitude
double xPixelLocation = Math.Cos(angleFlat) * (magnitudeFlat / edgeRadius) * (UsableLateralAmplitude - SurfaceBubble.Width) / 2;
double yPixelLocation = Math.Sin(angleFlat) * (magnitudeFlat / edgeRadius) * (UsableLateralAmplitude - SurfaceBubble.Width) / 2;
SurfaceBubble.SetValue(Canvas.LeftProperty, xPixelLocation + (SurfaceLevelOuter.Width - SurfaceBubble.Width) / 2);
SurfaceBubble.SetValue(Canvas.TopProperty, yPixelLocation + (SurfaceLevelOuter.Height - SurfaceBubble.Width) / 2);
// Change bubble shape
double stretchRatio;
double horizontalDirection;
if (atEdge)
{
stretchRatio = MaximumBubbleXYStretchRatio;
horizontalDirection = angleFlat - Math.PI / 2;
}
else
{
x = _bubbleSpeed.X;
y = _bubbleSpeed.Y;
double horizontalSpeed = Math.Sqrt(x * x + y * y);
horizontalDirection = Math.Atan2(y, x) - Math.PI / 2;
stretchRatio = Math.Min(horizontalSpeed * SpeedBubbleStrechingCoef, MaximumBubbleXYStretchRatio - 1.0) + 1.0;
}
SurfaceBubbleDirection.Angle = horizontalDirection * 180.0 / Math.PI;
SurfaceBubbleScale.ScaleX = stretchRatio;
SurfaceBubbleScale.ScaleY = 1.0 / stretchRatio;
}
/// <summary>
/// Called on accelerometer sensor sample available.
/// Main accelerometer data filtering routine
/// </summary>
/// <param name = "sender">Sender of the event.</param>
/// <param name = "e">AccelerometerReadingAsyncEventArgs</param>
private void SensorReadingChanged(object sender, AccelerometerReadingEventArgs e)
{
Simple3DVector lowPassFilteredAcceleration;
Simple3DVector optimalFilteredAcceleration;
Simple3DVector averagedAcceleration;
var rawAcceleration = new Simple3DVector(e.X, e.Y, e.Z);
lock (this.sampleBuffer)
{
if (!this.initialized)
{
// Initialize file with 1st value
this.sampleSum = rawAcceleration * SamplesCount;
this.averageAcceleration = rawAcceleration;
// Initialize file with 1st value
for (int i = 0; i < SamplesCount; i++)
{
this.sampleBuffer[i] = this.averageAcceleration;
}
this.previousLowPassOutput = this.averageAcceleration;
this.previousOptimalFilterOutput = this.averageAcceleration;
this.initialized = true;
}
// low-pass filter
lowPassFilteredAcceleration =
new Simple3DVector(
LowPassFilter(rawAcceleration.X, this.previousLowPassOutput.X),
LowPassFilter(rawAcceleration.Y, this.previousLowPassOutput.Y),
LowPassFilter(rawAcceleration.Z, this.previousLowPassOutput.Z));
this.previousLowPassOutput = lowPassFilteredAcceleration;
// optimal filter
optimalFilteredAcceleration =
new Simple3DVector(
FastLowAmplitudeNoiseFilter(rawAcceleration.X, this.previousOptimalFilterOutput.X),
FastLowAmplitudeNoiseFilter(rawAcceleration.Y, this.previousOptimalFilterOutput.Y),
FastLowAmplitudeNoiseFilter(rawAcceleration.Z, this.previousOptimalFilterOutput.Z));
this.previousOptimalFilterOutput = optimalFilteredAcceleration;
// Increment circular buffer insertion index
this.sampleIndex++;
if (this.sampleIndex >= SamplesCount)
{
this.sampleIndex = 0;
// if at max SampleCount then wrap samples back to the beginning position in the list
}
// Add new and remove old at _sampleIndex
Simple3DVector newVect = optimalFilteredAcceleration;
this.sampleSum += newVect;
this.sampleSum -= this.sampleBuffer[this.sampleIndex];
this.sampleBuffer[this.sampleIndex] = newVect;
averagedAcceleration = this.sampleSum / SamplesCount;
this.averageAcceleration = averagedAcceleration;
// Stablity check
// If current low-pass filtered sample is deviating for more than 1/100 g from average (max of 0.5 deg inclination noise if device steady)
// then reset the stability counter.
// The calibration will be prevented until the counter is reaching the sample count size (calibration enabled only if entire
// sampling buffer is "stable"
Simple3DVector deltaAcceleration = averagedAcceleration - optimalFilteredAcceleration;
if ((Math.Abs(deltaAcceleration.X) > maximumStabilityDeltaOffset) ||
(Math.Abs(deltaAcceleration.Y) > maximumStabilityDeltaOffset) ||
(Math.Abs(deltaAcceleration.Z) > maximumStabilityDeltaOffset))
{
// Unstable
this.deviceStableCount = 0;
}
else
{
if (this.deviceStableCount < SamplesCount)
{
++this.deviceStableCount;
}
}
}
if (this.ReadingChanged != null)
{
var readingEventArgs = new AccelerometerHelperReadingEventArgs
{
RawAcceleration = rawAcceleration,
LowPassFilteredAcceleration = lowPassFilteredAcceleration,
OptimalyFilteredAcceleration = optimalFilteredAcceleration,
AverageAcceleration = averagedAcceleration
};
this.ReadingChanged(this, readingEventArgs);
}
}
void _accelerometer_ReadingChanged(object sender, AccelerometerHelperReadingEventArgs e)
{
if (_last == null)
{
_last = e.RawAcceleration;
}
else
{
Simple3DVector _current = e.RawAcceleration;
double _cosa = (_current.X * _last.X + _current.Y * _last.Y + _current.Z * _last.Z) /
(Math.Sqrt(_current.X * _current.X + _current.Y * _current.Y + _current.Z * _current.Z) *
Math.Sqrt(_last.X * _last.X + _last.Y * _last.Y + _last.Z * _last.Z));
if (_samplesCount < 10)
{
_dsamples[_samplesCount] += _cosa;
_samplesCount += 1;
}
else
{
Array.Copy(_dsamples, 1, _dsamples, 0, _dsamples.Length - 1);
_dsamples[9] = _cosa;
if (_skip >= 10)
{
_skip = 0;
_isSkipping = false;
}
else
{
if (_isSkipping)
{
_skip += 1;
}
}
if (!_isSkipping)
{
//double _weightedAverage = (double)Enumerable.Range(0, 9).Select(x => (x + 2) * _dsamples[x]).Sum() / 55;
////don't know why +2 works but as long as it does...or not
//non-LINQ alternative
double _weightedAverage = 0;
for (int i = 0; i < 10; i++)
{
_weightedAverage += (i + 1) * _dsamples[i];
}
_weightedAverage = _weightedAverage / 55;
if (_weightedAverage > _lastAverage && _weightedAverage < 0.95) //hardcoded threshold
{
OnStepDetected();
_isSkipping = true;
_skip = 0;
}
_lastAverage = _weightedAverage;
}
}
_last = _current;
}
}
public DeviceOrientationInfo(double angle, int horizontalScreenAxisPolarity, Simple3DVector typicalGravityVector)
{
this.AngleOnXYPlan = angle;
this.HorizontalAxisPolarity = horizontalScreenAxisPolarity;
this.NormalGravityVector = typicalGravityVector;
}
public DeviceOrientationInfo(double angle, int horizontalScreenAxisPolarity, Simple3DVector typicalGravityVector)
{
AngleOnXYPlan = angle;
HorizontalAxisPolarity = horizontalScreenAxisPolarity;
NormalGravityVector = typicalGravityVector;
}
