/// <summary>
/// Initializes a new instance of the <see cref="Trame.Implementation.Skeleton.OrientedJoint"/> class.
/// </summary>
/// <param name="type">Type.</param>
/// <param name="valid">If set to <c>true</c> valid.</param>
public OrientedJoint(JointType type, bool valid)
{
this.type = type;
this.isValid = valid;
orientation = new Vector4();
point = new Vector3();
}
public void UpdateSkeleton(JointType jt, IJoint j)
{
lock (_joints)
{
_joints[jt] = j;
}
}
public void Add(Skeleton skeleton, JointType jointType)
{
var trackingID = skeleton.TrackingId;
var position = skeleton.Joints.Where(j => j.JointType == jointType).First().Position.ToVector3();
Add(position, trackingID);
}
private void DrawBone(KinectSensor sensor, Skeleton skeleton, DrawingContext drawingContext, JointType jointType0, JointType jointType1)
{
Joint joint0 = skeleton.Joints[jointType0];
Joint joint1 = skeleton.Joints[jointType1];
// If we can't find either of these joints, exit
if (joint0.TrackingState == JointTrackingState.NotTracked ||
joint1.TrackingState == JointTrackingState.NotTracked)
{
return;
}
// Don't draw if both points are inferred
if (joint0.TrackingState == JointTrackingState.Inferred &&
joint1.TrackingState == JointTrackingState.Inferred)
{
return;
}
// We assume all drawn bones are inferred unless BOTH joints are tracked
Pen drawPen = GetInferredPen(sensor);
if (joint0.TrackingState == JointTrackingState.Tracked && joint1.TrackingState == JointTrackingState.Tracked)
{
drawPen = GetTrackedPen(sensor);
}
drawingContext.DrawLine(drawPen, Transform(sensor, joint0.Position), Transform(sensor, joint1.Position));
}
/// <summary>
/// Calculates the relative velocity of a joint referencing to a second one.</summary>
/// <param name="steady">
/// The referenctial JointType.</param>
/// <param name="moving">
/// The moving JointType of interest</param>
/// <returns>
/// Returns the relative velocity in meters</returns>
public static double GetRelativeVelocity(JointType steady, JointType moving)
{
if (!(MainWindow.jointRecords.PositionExistsAt(steady, 0) && MainWindow.jointRecords.PositionExistsAt(steady, 1) &&
MainWindow.jointRecords.PositionExistsAt(steady, 2)
&& MainWindow.jointRecords.PositionExistsAt(moving, 0) && MainWindow.jointRecords.PositionExistsAt(moving, 1)
&& MainWindow.jointRecords.PositionExistsAt(moving, 2) ))
{
return 0;
}
CameraSpacePoint d0 = SubstractedPointsAt(steady, moving, 0);
CameraSpacePoint d1 = SubstractedPointsAt(steady, moving, 1);
if (!(MainWindow.jointRecords.PositionExistsAt(steady, 3) && MainWindow.jointRecords.PositionExistsAt(steady, 3)))
{
return DistanceBetweenPoints(d0, d1) / MainWindow.jointRecords.MillisBetweenPositions(moving, 1, 0);
}
CameraSpacePoint d2 = SubstractedPointsAt(steady, moving, 2);
CameraSpacePoint d3 = SubstractedPointsAt(steady, moving, 3);
return Median(
DistanceBetweenPoints(d0, d1) * 1000.0 / MainWindow.jointRecords.MillisBetweenPositions(moving, 1, 0),
DistanceBetweenPoints(d1, d2) * 1000.0 / MainWindow.jointRecords.MillisBetweenPositions(moving, 2, 1),
DistanceBetweenPoints(d2, d3) * 1000.0 / MainWindow.jointRecords.MillisBetweenPositions(moving, 3, 2)
);
}
public static void DesenharMaosUsuario(
this SkeletonFrame pQuadro,
KinectSensor pKinectSensor,
Canvas pCanvasParaDesenhar,
Brush pCorDesenho,
JointType pMaoParaDesenhar)
{
if (pKinectSensor == null)
throw new ArgumentNullException("kinectSensor");
if (pCanvasParaDesenhar == null)
throw new ArgumentNullException("canvasParaDesenhar");
Skeleton lEsqueleto = ObterEsqueletoUsuario(pQuadro);
if (lEsqueleto != null)
{
EsqueletoUsuarioAuxiliar lEsqueletoUsuarioAuxiliar = new EsqueletoUsuarioAuxiliar(pKinectSensor);
//desenha mao direita
//lEsqueletoUsuarioAuxiliar.DesenharUmaMaoPintura(
// lEsqueleto.Joints[JointType.HandRight],
// pCanvasParaDesenhar,
// Brushes.Aquamarine);
//desenha mao esquerda
lEsqueletoUsuarioAuxiliar.DesenharUmaMaoPintura(
//lEsqueleto.Joints[JointType.HandRight],
lEsqueleto.Joints[pMaoParaDesenhar],
pCanvasParaDesenhar,
pCorDesenho);
}
}
public SwipeCondition(Person p, JointType leftOrRightHand)
: base(p)
{
_index = 0;
_hand = leftOrRightHand;
_checker = new Checker(p);
}
// MainWindow Constructor
public MainWindow()
{
InitializeComponent();
// Instantiate the wave player
_player = new SineWavePlayer();
// Set up the UI
freqLabel.Content = _player.Frequency;
ampLabel.Content = _player.Amplitude;
for (int i = 0; i < _player.frequencyDict.Count; i++)
{
keyBox.Items.Add(_player.frequencyDict.ElementAt(i).Key);
}
keyBox.SelectedIndex = 0;
// Initialize the wave player
ChangeKey();
_player.Frequency = _player.MinFreq;
// Set the starting handedness
_freqHand = JointType.HandRight;
_ampHand = JointType.HandLeft;
// Draw the guides
_freqInterval = guideCanvas.Height / 12;
//_ampInterval = guideCanvas.Height / 4;
DrawGuides();
// Instantiate and initialize the KinectHelper
_helper = new KinectHelper(true, false, true);
_helper.ToggleSeatedMode(true);
_helper.SkeletonDataChanged += new KinectHelper.SkeletonDataChangedEvent(SkeletonDataChange);
skeletonImage.Source = _helper.skeletonBitmap;
rgbImage.Source = _helper.colorBitmap;
}
void Plot(JointType centerID, JointType baseID, JointCollection joints)
{
float centerX;
float centerY;
GetCoordinates(centerID, joints, out centerX, out centerY);
float baseX;
float baseY;
GetCoordinates(baseID, joints, out baseX, out baseY);
double diameter = Math.Abs(baseY - centerY);
Ellipse ellipse = new Ellipse
{
Width = diameter,
Height = diameter,
HorizontalAlignment = HorizontalAlignment.Left,
VerticalAlignment = VerticalAlignment.Top,
StrokeThickness = 4.0,
Stroke = new SolidColorBrush(Colors.Green),
StrokeLineJoin = PenLineJoin.Round
};
Canvas.SetLeft(ellipse, centerX - ellipse.Width / 2);
Canvas.SetTop(ellipse, centerY - ellipse.Height / 2);
rootCanvas.Children.Add(ellipse);
}
// Cache here per time step to reduce cache misses.
// Vec2 m_localCenterA, m_localCenterB;
// float m_invMassA, m_invIA;
// float m_invMassB, m_invIB;
protected Joint(IWorldPool worldPool, JointDef def)
{
Debug.Assert(def.bodyA != def.bodyB);
pool = worldPool;
m_type = def.type;
m_prev = null;
m_next = null;
m_bodyA = def.bodyA;
m_bodyB = def.bodyB;
m_collideConnected = def.collideConnected;
m_islandFlag = false;
m_userData = def.userData;
m_edgeA = new JointEdge();
m_edgeA.joint = null;
m_edgeA.other = null;
m_edgeA.prev = null;
m_edgeA.next = null;
m_edgeB = new JointEdge();
m_edgeB.joint = null;
m_edgeB.other = null;
m_edgeB.prev = null;
m_edgeB.next = null;
// m_localCenterA = new Vec2();
// m_localCenterB = new Vec2();
}
/// <summary>
/// Create a Kalman smoothing joint with default configuration values.
/// </summary>
/// <param name="jointType">The joint type to create</param>
public KalmanJoint(JointType jointType)
: base(jointType)
{
var parms = new KalmanSmoothingParameters();
_measurementUncertainty = new Vector3(parms.MeasurementUncertainty);
_jitterRadius = parms.JitterRadius;
}
public PoseAngle(JointType centerJoint, JointType angleJoint, double angle, double threshold)
{
CenterJoint = centerJoint;
AngleJoint = angleJoint;
Angle = angle;
Threshold = threshold;
}
/// <summary>
///
/// </summary>
/// <param name="jointType"></param>
/// <param name="threshold"></param>
/// <param name="comparator"></param>
/// <param name="name"></param>
/// <param name="key"></param>
protected AThresholdGesture(JointType jointType, double threshold, ThresholdComparator comparator, string name, Key key)
: base(name, key)
{
this.jointType = jointType;
this.threshold = threshold;
this.comparator = comparator;
}
public bool PositionExistsAt(JointType joint, int n)
{
return (previousPositionsOf.ContainsKey(joint)) &&
(n <= previousPositionsOf[joint].Count - 1) &&
(n >= 0) &&
(previousPositionsOf[joint][previousPositionsOf[joint].Count - n - 1] != null);
}
/// <summary>
/// Draws a bone line between two joints
/// </summary>
/// <param name="skeleton">skeleton to draw bones from</param>
/// <param name="drawingContext">drawing context to draw to</param>
/// <param name="jointType0">joint to start drawing from</param>
/// <param name="jointType1">joint to end drawing at</param>
private static void DrawBone(Skeleton skeleton, Graphics drawingContext, JointType jointType0, JointType jointType1)
{
Joint joint0 = skeleton.Joints[jointType0];
Joint joint1 = skeleton.Joints[jointType1];
// If we can't find either of these joints, exit
if (joint0.TrackingState == JointTrackingState.NotTracked ||
joint1.TrackingState == JointTrackingState.NotTracked)
{
return;
}
// Don't draw if both points are inferred
if (joint0.TrackingState == JointTrackingState.Inferred &&
joint1.TrackingState == JointTrackingState.Inferred)
{
return;
}
// We assume all drawn bones are inferred unless BOTH joints are tracked
Pen drawPen = new Pen(Brushes.Gray, 1);
if (joint0.TrackingState == JointTrackingState.Tracked && joint1.TrackingState == JointTrackingState.Tracked)
{
drawPen = new Pen(Brushes.Green, 6);
}
drawingContext.DrawLine(drawPen,
(int)SkeletonPointToScreen(joint0.Position).X,
(int)SkeletonPointToScreen(joint0.Position).Y,
(int)SkeletonPointToScreen(joint1.Position).X,
(int)SkeletonPointToScreen(joint1.Position).Y);
}
public KinectDataPoint getDataPoint(JointType type, KinectUser user)
{
KinectDataPoint result = null;
jointMap.TryGetValue(type, out result);
return result;
}
public int readarmangle(Skeleton[] skeldata, JointType joint1, JointType joint2, int ang1)
{
ang1 = 0;
Vector2 joint1pos = new Vector2(0, 0);
Vector2 joint2pos = new Vector2(0, 0);
int ang = 0;
foreach (Skeleton data in skeldata)
{
if (data.TrackingState == SkeletonTrackingState.Tracked)
{
foreach (Joint joint in data.Joints)
{
if (joint.JointType == joint1)
{
joint1pos.X = joint.Position.X;
joint1pos.Y = joint.Position.Y;
}
if (joint.JointType == joint2)
{
joint2pos.X = joint.Position.X;
joint2pos.Y = joint.Position.Y;
}
}
Vector2 anglevec = joint1pos - joint2pos;
ang = (int)MathHelper.ToDegrees((float)Math.Atan2(anglevec.X, anglevec.Y));
if (ang < 0) ang += 360;
}
}
return ang;
}
public void GestureInProgress(long userId, int userIndex, KinectGestures.Gestures gesture,
float progress, JointType joint, Vector3 screenPos)
{
//GestureInfo.guiText.text = string.Format("{0} Progress: {1:F1}%", gesture, (progress * 100));
// if(gesture == KinectGestures.Gestures.Click && progress > 0.3f)
// {
// string sGestureText = string.Format ("{0} {1:F1}% complete", gesture, progress * 100);
// if(GestureInfo != null)
// GestureInfo.guiText.text = sGestureText;
//
// progressDisplayed = true;
// }
// else
if((gesture == KinectGestures.Gestures.ZoomOut || gesture == KinectGestures.Gestures.ZoomIn) && progress > 0.5f)
{
string sGestureText = string.Format ("{0} detected, zoom={1:F1}%", gesture, screenPos.z * 100);
if(GestureInfo != null)
GestureInfo.guiText.text = sGestureText;
progressDisplayed = true;
}
else if(gesture == KinectGestures.Gestures.Wheel && progress > 0.5f)
{
string sGestureText = string.Format ("{0} detected, angle={1:F1} deg", gesture, screenPos.z);
if(GestureInfo != null)
GestureInfo.guiText.text = sGestureText;
progressDisplayed = true;
}
}
public BoneInfo(JointType jointType, int boneIndex, int parentIndex)
{
JointType = jointType;
Name = jointType.ToString();
BoneIndex = boneIndex;
ParentIndex = parentIndex;
}
/// <summary>
/// constructor with frames and threshold
/// </summary>
/// <param name="frames">framesToCompare</param>
/// <param name="thresh">threshold</param>
/// <param name="jointToTrack">default joint</param>
public GestureRecognizer(int frames, double threshold, JointType jointToTrack)
{
_framesToCompare = frames;
_threshold = threshold;
_trackedJoint = jointToTrack;
_coordinatesList = new List<SkeletonPoint>();
}
public AdjacentJointPair(JointType left, JointType right, double thresholdHeight)
{
JointTypes = Tuple.Create(left, right);
ThresholdHeight = thresholdHeight;
DeactivateJointPair();
}
/// <summary>
/// default constructor with joint
/// </summary>
/// <param name="jointToTrack">default joint</param>
public GestureRecognizer(JointType jointToTrack)
{
_framesToCompare = 20;
_threshold = 10;
_trackedJoint = jointToTrack;
_coordinatesList = new List<SkeletonPoint>();
}
// returns the parent joint of the given joint
public static JointType GetParentJoint(JointType joint)
{
switch (joint)
{
case JointType.SpineBase:
return JointType.SpineBase;
case JointType.Neck:
return JointType.SpineShoulder;
case JointType.SpineShoulder:
return JointType.SpineBase;
case JointType.ShoulderLeft:
case JointType.ShoulderRight:
return JointType.SpineShoulder;
case JointType.HipLeft:
case JointType.HipRight:
return JointType.SpineBase;
case JointType.HandTipLeft:
case JointType.ThumbLeft:
return JointType.HandLeft;
case JointType.HandTipRight:
case JointType.ThumbRight:
return JointType.HandRight;
}
return (JointType)((int)joint - 1);
}
public ConnectedJoint(JointType jointType, int jointID)
{
this.jointType = jointType;
this.jointID = jointID;
this.nextJoint = null;
this.childGestureRulesCollection = new List<ChildGestureRules>();
}
internal void checkDrumHit(MainWindow.Player player, JointType joint)
{
//checkDrumHit code
//MessageBox.Show(Convert.ToString(hitAreaStart[0][1]));
if (player.skeleton != null && player.skeleton.Joints[joint].TrackingState == JointTrackingState.Tracked)
{
double posX = player.skeleton.Joints[joint].Position.X;
double posY = player.skeleton.Joints[joint].Position.Y;
double posZ = player.skeleton.Joints[joint].Position.Z;
for (int i = 0; i <= hitArea[player.skeleton.TrackingId].Count - 1; i++)
{
if (hitArea[player.skeleton.TrackingId][i].X1 < posX && hitArea[player.skeleton.TrackingId][i].X2 > posX && hitArea[player.skeleton.TrackingId][i].Y1 < posY && hitArea[player.skeleton.TrackingId][i].Y2 > posY && hitArea[player.skeleton.TrackingId][i].Z1 < posZ && hitArea[player.skeleton.TrackingId][i].Z2 > posZ)
{
if (!insideArea[player.skeleton.TrackingId][joint][i])
{
if (handMovements.difference != null)
{
if (handMovements.difference[player.skeleton.TrackingId][joint].Y < -0.01 || ((joint == JointType.FootLeft || joint == JointType.FootRight) && handMovements.difference[player.skeleton.TrackingId][joint].Z < -0.02))
{
hitDrum("drum" + i);
Debug.Print("HIT! " + i);
insideArea[player.skeleton.TrackingId][joint][i] = true;
}
}
}
}
else
{
insideArea[player.skeleton.TrackingId][joint][i] = false;
}
}
}
}
private void DrawBone(Skeleton skeleton, DrawingContext drawingContext, JointType jointType0,
JointType jointType1)
{
var joint0 = skeleton.Joints[jointType0];
var joint1 = skeleton.Joints[jointType1];
// If we can't find either of these joints, exit
if (joint0.TrackingState == JointTrackingState.NotTracked ||
joint1.TrackingState == JointTrackingState.NotTracked)
{
return;
}
// Don't draw if both points are inferred
if (joint0.TrackingState == JointTrackingState.Inferred &&
joint1.TrackingState == JointTrackingState.Inferred)
{
return;
}
// We assume all drawn bones are inferred unless BOTH joints are tracked
Pen drawPen = _inferredBonePen;
if (joint0.TrackingState == JointTrackingState.Tracked && joint1.TrackingState == JointTrackingState.Tracked)
{
drawPen = _trackedBonePen;
}
drawingContext.DrawLine(drawPen, SkeletonPointToScreen(joint0.Position),
SkeletonPointToScreen(joint1.Position));
}
public TimedPosition GetNthMostRecentPosition(JointType joint, int n)
{
if (n > previousPositionsOf[joint].Count - 1 || n < 0)
{
return null;
}
return previousPositionsOf[joint][previousPositionsOf[joint].Count - n - 1];
}
public SkeletonPoint GetPosition(JointType jt)
{
if (TrackingState == SkeletonTrackingState.Tracked)
{
return joints[jt].Position;
}
return new SkeletonPoint();
}
public JointTrackingState GetState(JointType jt)
{
if (TrackingState == SkeletonTrackingState.Tracked)
{
return joints[jt].TrackingState;
}
return JointTrackingState.NotTracked;
}
public JointDef()
{
type = JointType.UNKNOWN;
userData = null;
bodyA = null;
bodyB = null;
collideConnected = false;
}
public virtual Pose After(JointType other) => one.After(other) & another.After(other);
/// <summary>
/// Requires two existing revolute or prismatic joints (any combination will work).
/// The provided joints must attach a dynamic body to a static body.
/// </summary>
/// <param name="jointA">The first joint.</param>
/// <param name="jointB">The second joint.</param>
/// <param name="ratio">The ratio.</param>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, float ratio = 1f)
{
JointType = JointType.Gear;
BodyA = bodyA;
BodyB = bodyB;
JointA = jointA;
JointB = jointB;
Ratio = ratio;
_typeA = jointA.JointType;
_typeB = jointB.JointType;
Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic || _typeA == JointType.FixedRevolute || _typeA == JointType.FixedPrismatic);
Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic || _typeB == JointType.FixedRevolute || _typeB == JointType.FixedPrismatic);
float coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
_bodyC = JointA.BodyA;
_bodyA = JointA.BodyB;
// Get geometry of joint1
Transform xfA = _bodyA._xf;
float aA = _bodyA._sweep.A;
Transform xfC = _bodyC._xf;
float aC = _bodyC._sweep.A;
if (_typeA == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)jointA;
_localAnchorC = revolute.LocalAnchorA;
_localAnchorA = revolute.LocalAnchorB;
_referenceAngleA = revolute.ReferenceAngle;
_localAxisC = Vector2.Zero;
coordinateA = aA - aC - _referenceAngleA;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)jointA;
_localAnchorC = prismatic.LocalAnchorA;
_localAnchorA = prismatic.LocalAnchorB;
_referenceAngleA = prismatic.ReferenceAngle;
_localAxisC = prismatic.LocalXAxis;
Vector2 pC = _localAnchorC;
Vector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p));
coordinateA = Vector2.Dot(pA - pC, _localAxisC);
}
_bodyD = JointB.BodyA;
_bodyB = JointB.BodyB;
// Get geometry of joint2
Transform xfB = _bodyB._xf;
float aB = _bodyB._sweep.A;
Transform xfD = _bodyD._xf;
float aD = _bodyD._sweep.A;
if (_typeB == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)jointB;
_localAnchorD = revolute.LocalAnchorA;
_localAnchorB = revolute.LocalAnchorB;
_referenceAngleB = revolute.ReferenceAngle;
_localAxisD = Vector2.Zero;
coordinateB = aB - aD - _referenceAngleB;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)jointB;
_localAnchorD = prismatic.LocalAnchorA;
_localAnchorB = prismatic.LocalAnchorB;
_referenceAngleB = prismatic.ReferenceAngle;
_localAxisD = prismatic.LocalXAxis;
Vector2 pD = _localAnchorD;
Vector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p));
coordinateB = Vector2.Dot(pB - pD, _localAxisD);
}
_ratio = ratio;
_constant = coordinateA + _ratio * coordinateB;
_impulse = 0.0f;
}
public virtual Pose Before(JointType other) => one.Before(other) & another.Before(other);
void UpdateJoint(Body body, JointType jt, TransformSmoothParameters smoothingParams)
{
CameraSpacePoint vFilteredPosition;
CameraSpacePoint vDiff;
CameraSpacePoint vTrend;
float fDiff;
var joint = body.Joints[jt];
var vRawPosition = joint.Position;
var vPrevFilteredPosition = mPHistory[(int)jt].MvFilteredPosition;
var vPrevTrend = mPHistory[(int)jt].MvTrend;
var vPrevRawPosition = mPHistory[(int)jt].MvRawPosition;
var bJointIsValid = JointPositionIsValid(vRawPosition);
// If joint is invalid, reset the filter
if (!bJointIsValid)
{
mPHistory[(int)jt].MDwFrameCount = 0;
}
// Initial start values
if (mPHistory[(int)jt].MDwFrameCount == 0)
{
vFilteredPosition = vRawPosition;
vTrend = CsVectorZero();
mPHistory[(int)jt].MDwFrameCount++;
}
else if (mPHistory[(int)jt].MDwFrameCount == 1)
{
vFilteredPosition = CsVectorScale(CsVectorAdd(vRawPosition, vPrevRawPosition), 0.5f);
vDiff = CsVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = CsVectorAdd(CsVectorScale(vDiff, smoothingParams.FCorrection),
CsVectorScale(vPrevTrend, 1.0f - smoothingParams.FCorrection));
mPHistory[(int)jt].MDwFrameCount++;
}
else
{
// First apply jitter filter
vDiff = CsVectorSubtract(vRawPosition, vPrevFilteredPosition);
fDiff = CsVectorLength(vDiff);
if (fDiff <= smoothingParams.FJitterRadius)
{
vFilteredPosition = CsVectorAdd(CsVectorScale(vRawPosition, fDiff / smoothingParams.FJitterRadius),
CsVectorScale(vPrevFilteredPosition,
1.0f - fDiff / smoothingParams.FJitterRadius));
}
else
{
vFilteredPosition = vRawPosition;
}
// Now the double exponential smoothing filter
vFilteredPosition = CsVectorAdd(CsVectorScale(vFilteredPosition, 1.0f - smoothingParams.FSmoothing),
CsVectorScale(CsVectorAdd(vPrevFilteredPosition, vPrevTrend),
smoothingParams.FSmoothing));
vDiff = CsVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = CsVectorAdd(CsVectorScale(vDiff, smoothingParams.FCorrection),
CsVectorScale(vPrevTrend, 1.0f - smoothingParams.FCorrection));
}
// Predict into the future to reduce latency
var vPredictedPosition = CsVectorAdd(vFilteredPosition, CsVectorScale(vTrend, smoothingParams.FPrediction));
// Check that we are not too far away from raw data
vDiff = CsVectorSubtract(vPredictedPosition, vRawPosition);
fDiff = CsVectorLength(vDiff);
if (fDiff > smoothingParams.FMaxDeviationRadius)
{
vPredictedPosition =
CsVectorAdd(CsVectorScale(vPredictedPosition, smoothingParams.FMaxDeviationRadius / fDiff),
CsVectorScale(vRawPosition, 1.0f - smoothingParams.FMaxDeviationRadius / fDiff));
}
// Save the data from this frame
mPHistory[(int)jt].MvRawPosition = vRawPosition;
mPHistory[(int)jt].MvFilteredPosition = vFilteredPosition;
mPHistory[(int)jt].MvTrend = vTrend;
// Output the data
mPFilteredJoints[(int)jt] = vPredictedPosition;
}
public virtual Pose Above(JointType other) => one.Above(other) & another.Above(other);
public static double calAngle_3Joints(Skeleton s, JointType jt1, JointType jt2, JointType jt3)
{
try
{
double Angulo = 0;
double shrhX = s.Joints[jt1].Position.X - s.Joints[jt2].Position.X;
double shrhY = s.Joints[jt1].Position.Y - s.Joints[jt2].Position.Y;
double shrhZ = s.Joints[jt1].Position.Z - s.Joints[jt2].Position.Z;
double hsl = vectorNorm(shrhX, shrhY, shrhZ);
double unrhX = s.Joints[jt3].Position.X - s.Joints[jt2].Position.X;
double unrhY = s.Joints[jt3].Position.Y - s.Joints[jt2].Position.Y;
double unrhZ = s.Joints[jt3].Position.Z - s.Joints[jt2].Position.Z;
double hul = vectorNorm(unrhX, unrhY, unrhZ);
double mhshu = shrhX * unrhX + shrhY * unrhY + shrhZ * unrhZ;
double x = mhshu / (hul * hsl);
if (x != Double.NaN)
{
if (-1 <= x && x <= 1)
{
double angleRad = Math.Acos(x);
Angulo = angleRad * (180.0 / Math.PI);
}
else
{
Angulo = 0;
}
}
else
{
Angulo = 0;
}
return(Angulo);
}
catch (Exception e) { TheSys.showError("Err cal3D_Temp: " + e.ToString(), true); }
return(0);
}
/// <summary>
/// Dibuja una línea osea entre dos articulaciones.
/// </summary>
/// <param name="skeleton">esqueleto para extraer huesos</param>
/// <param name="drawingContext">contexto de dibujo para dibujar</param>
/// <param name="jointType0">conjunta para comenzar a dibujar desde</param>
/// <param name="jointType1"> articulación para terminar el dibujo en</param>
private void DrawBone(Skeleton skeleton, DrawingContext drawingContext, JointType jointType0, JointType jointType1)
{
Joint joint0 = skeleton.Joints[jointType0];
Joint joint1 = skeleton.Joints[jointType1];
// Si no podemos encontrar ninguna de estas articulaciones, salga
if (joint0.TrackingState == JointTrackingState.NotTracked || joint1.TrackingState == JointTrackingState.NotTracked)
{
return;
}
// No dibuje si se infieren ambos puntos
if (joint0.TrackingState == JointTrackingState.Inferred && joint1.TrackingState == JointTrackingState.Inferred)
{
return;
}
// Suponemos que todos los huesos extraídos se infieren a menos que se rastreen AMBAS articulaciones
Pen drawPen = this.NoHuesoLapicero;
if (joint0.TrackingState == JointTrackingState.Tracked && joint1.TrackingState == JointTrackingState.Tracked)
{
drawPen = this.SeguimientoHuesoLapicero;
}
drawingContext.DrawLine(drawPen, this.SkeletonPointToScreen(joint0.Position), this.SkeletonPointToScreen(joint1.Position));
}
public JointSelector(JointType joint)
{
Joint = joint;
}
public virtual Posture OverlapsWith(JointType other)
{
return(new FunctionalPosture(body =>
RatioCalculator.DifferenceDistance(body.XDiff(Joint, other), body.YDiff(Joint, other), body.ZDiff(Joint, other))));
}
/// <summary>
/// Draws one bone of a body (joint to joint)
/// </summary>
/// <param name="joints">joints to draw</param>
/// <param name="jointPoints">translated positions of joints to draw</param>
/// <param name="jointType0">first joint of bone to draw</param>
/// <param name="jointType1">second joint of bone to draw</param>
/// <param name="drawingContext">drawing context to draw to</param>
/// /// <param name="drawingPen">specifies color to draw a specific bone</param>
private void DrawBone(IReadOnlyDictionary <JointType, Joint> joints, IDictionary <JointType, Point> jointPoints, JointType jointType0, JointType jointType1, DrawingContext drawingContext, Pen drawingPen)
{
Joint joint0 = joints[jointType0];
Joint joint1 = joints[jointType1];
// If we can't find either of these joints, exit
if (joint0.TrackingState == TrackingState.NotTracked ||
joint1.TrackingState == TrackingState.NotTracked)
{
return;
}
// We assume all drawn bones are inferred unless BOTH joints are tracked
Pen drawPen = this.inferredBonePen;
if ((joint0.TrackingState == TrackingState.Tracked) && (joint1.TrackingState == TrackingState.Tracked))
{
drawPen = drawingPen;
}
drawingContext.DrawLine(drawPen, jointPoints[jointType0], jointPoints[jointType1]);
}
public virtual Pose AtTheSameHeightOf(JointType other) => one.AtTheSameHeightOf(other) & another.AtTheSameHeightOf(other);
public virtual Pose AtTheSameLengthOf(JointType other) => one.AtTheSameLengthOf(other) & another.AtTheSameLengthOf(other);
public virtual Pose Below(JointType other) => one.Below(other) & another.Below(other);
public virtual Pose ToTheRightOf(JointType other) => one.ToTheRightOf(other) & another.ToTheRightOf(other);
public static string GetJointName(JointType jointType)
{
return(GetJointName((int)jointType));
}
//TODO: At some point, it could be necessary to unite this class with JointSelector
// using a base class ofr interface.
public JointPairSelector(JointType one, JointType another) :
this(new JointSelector(one), new JointSelector(another))
{
}
