/// <summary>
/// Stores virtual accleration data into buffer
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void OnIdealSensorNewTData(object sender, VirtualSensorData e)
{
//TO DO: IF data gets pulled into buffer, gets tagged with section ID? Wait.. thought we were tagging all data touched??
//regardless, no longer doing this here.
//e.section = this.CalibrationCount;
//DataTracker.CurrentSection = DataTracker.SectionCounter;
//derivation and store into buffer
AccelerationTime t = new AccelerationTime();
if (ICherryPicker.isDataGood(e.acceleration, !e.isinferredornottracked))
{
//IF THIS CONDITIONAL IS CHANGED, IT MUST ALSO BE CHANGED IN THE DATA PRODUCERS.
t.setVal(e.acceleration, e.NowInTicks, !e.isinferredornottracked); //if it's good, assign the proper current value - maybe we need to move this if statement to sensor producer
virtualAcc.Enqueue(t);
LastVirtualFiltered = false;
}
else
{
LastVirtualFiltered = true;
//DataTracker.CurrentSection = 0;
}
}
private void OnNewTData(object sender, KinectData e)
{
double[] virtualAcc = new double[] { 0, 0, 0 };
double[] virtualVel = new double[] { 0, 0, 0 };
double[] pos = new double[] { e.Joints[FurtherJoint].Position.X, e.Joints[FurtherJoint].Position.Y, e.Joints[FurtherJoint].Position.Z };
TrackingState trackingstate = e.Body.Joints[this.FurtherJoint].TrackingState;
//= MathFunctions.midpoint(e.Joints[FurtherJoint], e.Joints[CloserJoint], 100);
X_Filter.UpdateVal(pos[0], e.NowInTicks);
Y_Filter.UpdateVal(pos[1], e.NowInTicks);
Z_Filter.UpdateVal(pos[2], e.NowInTicks);
//acceleration
var temp = X_Filter.GetNDerivative(nd);
virtualAcc[0] = temp.Item1;
virtualAcc[1] = Y_Filter.GetNDerivative(nd).Item1 + 9.81;
//hack make z -
virtualAcc[2] = -Z_Filter.GetNDerivative(nd).Item1;
//position
double[] virtualPos = new double[] { pos[0], pos[1], pos[2] };
vsd = new VirtualSensorData();
vsd.NowInTicks = temp.Item2;
//vsd.NowInTicks = e.NowInTicks;
for (int i = 0; i < 3; i++)
{
vsd.acceleration[i] = virtualAcc[i];
vsd.velocity[i] = virtualVel[i];
vsd.position[i] = virtualPos[i];
}
//Rotation
vsd.rot = e.Body.JointOrientations[JointType.WristRight].Orientation.ToQuaternion(); //Apply filter under this note: X,Y,Z,W
//Begin Quat Filter
#region QuatFilter
double[] quat = new double[] { vsd.rot.W, vsd.rot.X, vsd.rot.Y, vsd.rot.Z };
//If this is the first data point, record that. The filter will then not be triggered on the first data point
//Assuming first data set is correct otherwise all values will be incorrectly flipped. Can improve conditionals later (delay for high confidence data, etc).
if (DataTracker.vsdFirstQuat[0] == 0 && DataTracker.vsdFirstQuat[1] == 0 && DataTracker.vsdFirstQuat[2] == 0 && DataTracker.vsdFirstQuat[3] == 0)
{
DataTracker.vsdFirstQuat = quat;
DataTracker.mvsdPrevQuat = quat;
}
int counter = 0;
for (int i = 0; i < 4; i++)
{
if (System.Math.Sign(DataTracker.mvsdPrevQuat[i]) != System.Math.Sign(quat[i]))
{
counter++;
}
}
if (counter == 4)
{
DataTracker.FlipCounter++;
for (int j = 0; j < 4; j++)
{
quat[j] = -quat[j];
}
}
//NOTE: just for right wrist, this can be generalized later. Only keep high confidence reference quaternions (except for first data point)
if (e.Body.Joints[JointType.ElbowRight].TrackingState == TrackingState.Tracked &&
e.Body.Joints[JointType.ShoulderRight].TrackingState == TrackingState.Tracked &&
e.Body.Joints[JointType.WristRight].TrackingState == TrackingState.Tracked)
{
for (int i = 0; i < 4; i++)
{
DataTracker.mvsdPrevQuat[i] = quat[i];
}
}
vsd.rot.W = quat[0];
vsd.rot.X = quat[1];
vsd.rot.Y = quat[2];
vsd.rot.Z = quat[3];
#endregion
//End Rotation Filter
if (this.FurtherJoint == JointType.WristRight)
{ //Only do strict tracking when right wrist is selected
TrackingState wristState = e.Body.Joints[JointType.WristRight].TrackingState;
HandState handState = e.Body.HandRightState;
TrackingState thumbState = e.Body.Joints[JointType.ThumbRight].TrackingState;
vsd.isinferredornottracked = (wristState != TrackingState.Tracked) || (thumbState != TrackingState.Tracked) || (handState != HandState.Open);
}
else
{
vsd.isinferredornottracked = (trackingstate == TrackingState.Inferred) || (trackingstate == TrackingState.NotTracked);
}
//If in Calibrator Setup (note: change calibrator setup to bool later? - 0 means not in setup) and the data is good, tag it.
if (ICherryPicker.isDataGood(vsd.acceleration, !vsd.isinferredornottracked)) //don't need to repeat this in mappedVirtual
{
vsd.section = DataTracker.CurrentSection;
DataTracker.ValidVSD = true;
}
else
{
vsd.section = 0;
DataTracker.ValidVSD = false;
}
if (this.NewIData != null)
{
this.NewIData(this, vsd); //[not edited] This triggers data collection? (So changes must be before this)
}
if (this.NewTData != null)
{
this.NewTData(this, vsd);
}
}
