private void CreateJoint(IList <string> lines, string line, IDictionary <string, IJoint> joints)
{
string[] parts = line.Split(' ');
IJoint joint = jointFactory.Create();
CommonCreateJoint(lines, joints, parts, joint);
}
public Relation(Person person1, Person person2, IJoint center1, IJoint center2, TimeSpan LocationTime)
{
Person1 = person1;
Person2 = person2;
this.Distance = distance(center1, center2);
this.LocationTime = LocationTime;
}
private void mouseMove(object sender, MouseEventArgs e)
{
if (isDragging)
{
dragDelta.x = e.X - dragStartPos.x;
dragDelta.y = e.Y - dragStartPos.y;
Invalidate();
}
else
{
Vector3D mousePos = new Vector3D(e.X, e.Y, 0);
if (hasHovered(skeleton.Root, mousePos))
{
Invalidate();
}
else
{
if (hoveredJoint != null)
{
hoveredJoint = null;
Invalidate();
}
}
}
}
public void InsertJoint(string parentName, IJoint joint)
{
// If we don't have a root accept everything.
if (Root == null)
{
Root = joint;
jointTable.Add(joint.Name, joint);
return;
}
if (jointTable.ContainsKey(joint.Name))
{
throw new Exception("Joint name '" + joint.Name + "' already exists.");
}
IJoint parent;
if (!jointTable.TryGetValue(parentName, out parent))
{
throw new Exception("Unknown parent '" + parentName + "'.");
}
parent.AddChild(joint);
jointTable.Add(joint.Name, joint);
}
public void UpdateSkeleton(JointType jt, IJoint j)
{
lock (_joints)
{
_joints[jt] = j;
}
}
public void UpdateSkeleton(JointType jt, IJoint j)
{
lock (_joints)
{
_joints[jt] = j;
}
}
private void CreateButton_Click(object sender, EventArgs e)
{
IJoint joint;
try
{
joint = structureProvider.Create(StructureTextBox.Text, settings, structures);
}
catch (Exception exception)
{
MessageBox.Show(exception.Message);
return;
}
if (worldRoot == null)
{
worldRoot = joint;
}
else
{
((IJoint)ObjectTreeView.SelectedNode.Tag).AddJoint(joint);
}
RefreshObjectTree();
}
private void MouseRightClick(HandState handRightState, IJoint rightHand, IJoint spineMid)
{
var zRightHandBodyDistance = spineMid.Position.Z - rightHand.Position.Z;
if (zRightHandBodyDistance > 0.5f) // Right Hand Moving Cursor
{
if (handRightState == HandState.Open && startRightHandClickGestureState == null)
{
startRightHandClickGestureState = new MouseGestureState(PHelper.CurrentTimeMillis());
startRightHandClickGestureState.oldCursorPosition = Cursor.Position;
}
else if (handRightState == HandState.Open && startRightHandClickGestureState != null)
{
var cursorMoveDistance = PointDistance(Cursor.Position, startRightHandClickGestureState.oldCursorPosition);
if (cursorMoveDistance < 8)
{
var timeDiff = PHelper.CurrentTimeMillis() - startRightHandClickGestureState.TimeStamp;
if (timeDiff > 2000)
{
MouseControl.DoMouseRightClick();
startRightHandClickGestureState = null;
}
}
else
{
startRightHandClickGestureState = null;
}
}
}
}
public IHand GetHand(HandType type, bool preferRight = true)
{
IJoint left = _joints[JointType.HAND_LEFT];
IJoint right = _joints[JointType.HAND_RIGHT];
switch (type)
{
case HandType.Left:
return(left.Valid ? left as IHand : null);
case HandType.Right:
return(left.Valid ? left as IHand : null);
default:
if (preferRight && right.Valid || !left.Valid && right.Valid)
{
return(right as IHand);
}
if (left.Valid)
{
return(left as IHand);
}
return(null);
}
}
private void mixJoints(IJoint result, IJoint joint1, double amount1, IJoint joint2, double amount2)
{
double enableJoint1 = 1.0;
double enableJoint2 = 1.0;
Vector3D p;
p = joint1.AnimationTransform * new Vector3D(1, 1, 1); // check, if AnimationTransform is Identity Matrix... for the lazy ones...
if (p.x == 1 && p.y == 1 && p.z == 1)
{
enableJoint1 = 0;
}
p = joint2.AnimationTransform * new Vector3D(1, 1, 1);
if (p.x == 1 && p.y == 1 && p.z == 1)
{
enableJoint2 = 0;
}
Matrix4x4 resultAnimationT = result.AnimationTransform;
Matrix4x4Utils.Blend(joint1.AnimationTransform, joint2.AnimationTransform, amount1 * enableJoint1, amount2 * enableJoint2, out resultAnimationT);
result.AnimationTransform = resultAnimationT;
for (int i = 0; i < result.Children.Count; i++)
{
mixJoints(result.Children[i], joint1.Children[i], amount1, joint2.Children[i], amount2);
}
}
private void MoveMouse(IBody body, IJoint lefthand, IJoint rightHand, IJoint spineMid)
{
//Map body Spine Mid - Shoulder Left/Right to center Screen
var zRightHandBodyDistance = spineMid.Position.Z - rightHand.Position.Z;
var zLeftHandBodyDistance = spineMid.Position.Z - lefthand.Position.Z;
//Console.WriteLine("(" + zLeftHandBodyDistance + "," + zRightHandBodyDistance + ")");
//Console.WriteLine(zLeftHandBodyDistance);
if (zRightHandBodyDistance > 0.45f && zLeftHandBodyDistance < 0.3f) // Right Hand Moving Cursor
{
RightHandActions(body, rightHand, spineMid);
}
else if (zRightHandBodyDistance < 0.3f && zLeftHandBodyDistance > 0.4f) // Left Hand Moving cursor
{
LeftHandActions(body, lefthand, spineMid);
}
else
{
// Do Nothing Maybe Lock Cursor Position
cursorSmoothing = CURSORSMOOTHING;
wasLeftGrip = false;
wasRightGrip = false;
alreadyTrackedPos = false;
}
}
private static IJoint BuildHand(Hand hand, IJoint wrist, int side)
{
var handJoint = new OrientedJoint();
if (hand.IsValid)
{
var palmNormal = hand.PalmNormal;
var handPosition = hand.PalmPosition;
handJoint.Orientation = new Vector4(10 * palmNormal.x, 10 * palmNormal.y, 10 * palmNormal.z, 0);
handJoint.Point = new Vector3(0, 0, -100);
var fingers = hand.Fingers;
foreach (var t in fingers)
{
var normal = t.Direction;
var position = t.TipPosition - handPosition;
handJoint.AddChild(CreateFinger(position, normal, FingerType2JointType(t.Type(), side)));
}
handJoint.Valid = true;
}
return handJoint;
}
/// <summary>
/// Creates the skeleton.
/// </summary>
/// <returns>The skeleton.</returns>
public static ISkeleton CreateSkeleton()
{
int centerY = 0;
int neckY = 400;
var centerOrientation = new Vector4(0, 0, 0, 0);
var s = new Skeleton {
Valid = true
};
var head = CreateHead();
IJoint neck = Creator.CreateParent(new List <IJoint> {
head
});
neck.Point = new Vector3(0, neckY, 0);
neck.JointType = JointType.NECK;
neck.Valid = true;
IJoint center = Creator.CreateParent(new List <IJoint> {
neck
});
center.Orientation = centerOrientation;
center.Point = new Vector3(0, centerY, 0);
center.JointType = JointType.CENTER;
center.Valid = true;
s.Root = center;
return(s);
}
protected override bool OnRunning(ActionResult actionResult, float dt)
{
if (currentWeapon.IsExpired)
{
this.activated = false;
this.currentWeapon = null;
this.currentJoint = null;
return(false);
}
buildUpDelay.Value += dt;
currentWeapon.LifeTime = (LifeSpan)weapons[stage].LifeTime.Clone();
if (buildUpDelay.IsFull)
{
buildUpDelay.Empty();
stage++;
if (stage < stageCount)
{
currentWeapon.IsExpired = true;
currentJoint.IsExpired = true;
SetStage(actionResult);
}
else
{
stage--;
}
}
return(true);
}
private void RefreshObjectTreeNode(IJoint joint, TreeNodeCollection nodes)
{
TreeNode node = new TreeNode(joint.ToString());
node.Tag = joint;
AddLocationTreeNode(node, joint.Location);
AddRotationNode(node, joint.Rotation);
AddZIndexNode(node, joint.ZIndex);
TreeNode elementsNode = new TreeNode("elements");
foreach (IElement childElement in joint.Elements)
{
TreeNode elementNode = new TreeNode(childElement.ToString());
AddPointsNode(elementNode, childElement.Points);
AddColorsNode(elementNode, childElement.Colors);
AddZIndexNode(elementNode, childElement.ZIndex);
elementNode.Tag = childElement;
elementsNode.Nodes.Add(elementNode);
}
node.Nodes.Add(elementsNode);
TreeNode jointsNode = new TreeNode("joints");
foreach (IJoint childJoint in joint.Joints)
{
RefreshObjectTreeNode(childJoint, jointsNode.Nodes);
}
node.Nodes.Add(jointsNode);
nodes.Add(node);
}
private void ChildRecursive(IJoint child, Func<IJoint, Vector3> same)
{
foreach (var c in child.GetChildren())
{
Assert.AreEqual(same(c), c.Point);
ChildRecursive(c, same);
}
}
public void AddChild(IJoint joint)
{
joint.Parent = this;
if (!Children.Exists(j => j.Name == joint.Name))
{
Children.Add(joint);
}
}
/// <summary>
/// Update (and filter) the joint position based on the referenced <c>IJoint</c>.
/// </summary>
public override void Update(IJoint joint)
{
_trackingState = joint.TrackingState;
if (_trackingState == TrackingState.NotTracked)
{
_position = new CameraSpacePoint();
return;
}
if (_init)
{
_init = false;
_filteredPosition = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
_positionVariance = new Vector3(1000f);
_velocityVariance = new Vector3(1000f);
}
else
{
var oldPosition = _filteredPosition;
var oldVelocity = _velocity;
// Predict
_filteredPosition = _filteredPosition + _velocity;
_velocity = _velocity + _velocityDelta;
_positionVariance = _positionVariance + _measurementUncertainty;
_velocityVariance = _velocityVariance + _measurementUncertainty;
// Update
var reportedPosition = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
// ... but first filter for jitter ...
var positionDelta = reportedPosition - oldPosition;
var differenceLength = Math.Abs((float)positionDelta.Length());
var jitterRadiusMod = joint.TrackingState == TrackingState.Tracked ? _jitterRadius : _jitterRadius * 2;
if (differenceLength <= jitterRadiusMod)
{
reportedPosition = (reportedPosition * (differenceLength / jitterRadiusMod)) +
(oldPosition * (1.0f - (differenceLength / jitterRadiusMod)));
}
_filteredPosition = (_positionVariance * reportedPosition + _measurementUncertainty * _filteredPosition) / (_measurementUncertainty + _positionVariance);
_velocity = (_velocityVariance * (reportedPosition - oldPosition) + _measurementUncertainty * _velocity) / (_measurementUncertainty + _velocityVariance);
_velocityDelta = _velocity - oldVelocity;
_positionVariance = Vector3.One / ((Vector3.One / _measurementUncertainty) + (Vector3.One / _positionVariance));
_velocityVariance = Vector3.One / ((Vector3.One / _measurementUncertainty) + (Vector3.One / _velocityVariance));
}
_position.X = _filteredPosition.X;
_position.Y = _filteredPosition.Y;
_position.Z = _filteredPosition.Z;
_depthPosition = new DepthSpacePoint();
_colorPosition = new ColorSpacePoint();
}
/// <summary>
/// Update (and filter) the joint position based on the referenced <c>IJoint</c>.
/// </summary>
public override void Update(IJoint joint)
{
_trackingState = joint.TrackingState;
if (_trackingState == TrackingState.NotTracked)
{
_position = new CameraSpacePoint();
return;
}
if (_init)
{
_init = false;
_filteredPosition = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
_positionVariance = new Vector3(1000f);
_velocityVariance = new Vector3(1000f);
}
else
{
var oldPosition = _filteredPosition;
var oldVelocity = _velocity;
// Predict
_filteredPosition = _filteredPosition + _velocity;
_velocity = _velocity + _velocityDelta;
_positionVariance = _positionVariance + _measurementUncertainty;
_velocityVariance = _velocityVariance + _measurementUncertainty;
// Update
var reportedPosition = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
// ... but first filter for jitter ...
var positionDelta = reportedPosition - oldPosition;
var differenceLength = Math.Abs((float)positionDelta.Length());
var jitterRadiusMod = joint.TrackingState == TrackingState.Tracked ? _jitterRadius : _jitterRadius * 2;
if (differenceLength <= jitterRadiusMod)
{
reportedPosition = (reportedPosition * (differenceLength / jitterRadiusMod)) +
(oldPosition * (1.0f - (differenceLength / jitterRadiusMod)));
}
_filteredPosition = (_positionVariance * reportedPosition + _measurementUncertainty * _filteredPosition) / (_measurementUncertainty + _positionVariance);
_velocity = (_velocityVariance * (reportedPosition - oldPosition) + _measurementUncertainty * _velocity) / (_measurementUncertainty + _velocityVariance);
_velocityDelta = _velocity - oldVelocity;
_positionVariance = Vector3.One / ((Vector3.One / _measurementUncertainty) + (Vector3.One / _positionVariance));
_velocityVariance = Vector3.One / ((Vector3.One / _measurementUncertainty) + (Vector3.One / _velocityVariance));
}
_position.X = _filteredPosition.X;
_position.Y = _filteredPosition.Y;
_position.Z = _filteredPosition.Z;
_depthPosition = new DepthSpacePoint();
_colorPosition = new ColorSpacePoint();
}
public bool IdentifyZoomGesture(IJoint rightHand, IJoint leftHand, IJoint spine, Boolean isLeftHandGripped, Boolean isRightHandGripped)
{
//If hands are below spine dont track gesture
if (GestureUtils.IsOneHandBelowSpine(rightHand, leftHand, spine))
{
State = 0;
return false;
}
if (!isLeftHandGripped || !isRightHandGripped)
{
State = 0;
return false;
}
var handInFront = (rightHand.Position.Z > leftHand.Position.Z) ? rightHand : leftHand;
float halfDistanceBetweenHandAndSpine = GestureUtils.HalfDistanceBetweenHandAndSpine(handInFront, spine);
if (GestureUtils.AreHandsSeparatedInZ(rightHand, leftHand, halfDistanceBetweenHandAndSpine) )
{
State = 0;
return false;
}
//If gesture is not being executed yet, then if hands are separated in Y, dont start it
if (State == 0)
{
if (GestureUtils.AreHandsSeparatedInY(rightHand, leftHand, MaximumHandsYDistanceToStart))
{
return false;
}
}
//Calculate and update hands distance
float distance = GestureUtils.DistanceXYBetweenPoints(rightHand.Position, leftHand.Position);
float deltaXDistance = distance - HandsDistance;
deltaXDistance = GestureUtils.NormalizeDistance(deltaXDistance, HandsDistanceErrorIgnored);
if (State == 0)
{
State = 1;
HandsDistance = distance;
return false;
}
//If got here than distance was updated and changed from last frame. Trigger gesture event to listeners.
if (deltaXDistance > 0)
{
Name = GestureEvents.ZOOM_IN;
HandsDistance = distance;
return true;
}
else if (deltaXDistance < 0)
{
Name = GestureEvents.ZOOM_OUT;
HandsDistance = distance;
return true;
}
return false;
}
private static void AddToJointTable(Dictionary <string, IJoint> jointTable, IJoint joint)
{
jointTable.Add(joint.Name, joint);
for (int i = 0; i < joint.Children.Count; i++)
{
IJoint j = joint.Children[i];
AddToJointTable(jointTable, j);
}
}
public NewJoint()
{
//the nodes constructor
//nothing to declare for this node
FRootJoint = new JointInfo();
FRootJoint.Id = 0;
FSkeleton = new Skeleton(FRootJoint);
FSkeleton.BuildJointTable();
FChildPins = new List <INodeIn>();
}
public bool IdentifyGrippedMove(IJoint rightHand, IJoint leftHand, IJoint spine, bool isRightHandGripped)
{
if (!isRightHandGripped)
{
State = 0;
return false;
}
float halfDistanceBetweenRightHandAndSpine = GestureUtils.HalfDistanceBetweenHandAndSpine(rightHand, spine);
if (GestureUtils.IsLeftHandCloseToRightOrInFront(rightHand, leftHand, halfDistanceBetweenRightHandAndSpine))
{
State = 0;
return false;
}
if (State == 0)
{
lastHandPosition = rightHand.Position;
State = 1;
}
SkeletonPoint currentPoint = rightHand.Position;
//If X grew, than hand moved right, else left
float xd = currentPoint.X - lastHandPosition.X;
if (xd > 0 && xd > DistanceToTriggerMove)
{
Name = GestureEvents.MOVE_RIGHT;
lastHandPosition.X += DistanceToTriggerMove;
return true;
}
if (xd < 0 && Math.Abs(xd) > DistanceToTriggerMove)
{
Name = GestureEvents.MOVE_LEFT;
lastHandPosition.X -= DistanceToTriggerMove;
return true;
}
//If Y grew, hand moved up, else down
float yd = currentPoint.Y - lastHandPosition.Y;
if (yd > 0 && yd > DistanceToTriggerMove)
{
Name = GestureEvents.MOVE_UP;
lastHandPosition.Y += DistanceToTriggerMove;
return true;
}
if (yd < 0 && Math.Abs(yd) > DistanceToTriggerMove)
{
Name = GestureEvents.MOVE_DOWN;
lastHandPosition.Y -= DistanceToTriggerMove;
return true;
}
return false;
}
private void copyAttributes(IJoint fromJoint, IJoint toJoint)
{
toJoint.AnimationTransform = fromJoint.AnimationTransform;
toJoint.BaseTransform = fromJoint.BaseTransform;
toJoint.Constraints = fromJoint.Constraints;
for (int i = 0; i < fromJoint.Children.Count; i++)
{
copyAttributes(fromJoint.Children[i], toJoint.Children[i]);
}
}
public bool AddChild(IJoint j)
{
try
{
children.Add(j.JointType, j);
}
catch (Exception ex)
{
return(false);
}
return(true);
}
public bool AddChild(IJoint j)
{
try
{
children.Add(j.JointType, j);
}
catch (Exception ex)
{
return false;
}
return true;
}
protected virtual void ReleaseWeapon()
{
currentJoint.IsExpired = true;
float distance = source.BoundingRadius + maxBoundingRadius + 5;
Vector2D direction = Vector2D.FromLengthAndAngle(1, source.Current.Position.Angular + offsetAngle);
Vector2D Velocity = Vector2D.FromLengthAndAngle(1, source.Current.Position.Angular + velocityAngle) * weapons[stage].MovementInfo.MaxLinearVelocity.Value;
currentWeapon.Current.Velocity.Linear += Velocity;
source.ApplyImpulse(distance * direction, Velocity * (-weapons[stage].MassInfo.Mass));
this.currentWeapon = null;
this.currentJoint = null;
}
/// <summary>
/// Update the joint position based on the referenced <c>IJoint</c>.
/// </summary>
public virtual void Update(IJoint joint)
{
if (this.JointType != joint.JointType)
{
throw new Exception("Cannot Update with Joint of a different Type");
}
_position = joint.Position;
_depthPosition = new DepthSpacePoint();
_colorPosition = new ColorSpacePoint();
_trackingState = joint.TrackingState;
}
public NewJoint()
{
//the nodes constructor
//nothing to declare for this node
rootJoint = new JointInfo();
rootJoint.Id = 0;
outputSkeleton = new Skeleton(rootJoint);
outputSkeleton.BuildJointTable();
childPins = new List <INodeIn>();
guids = new System.Guid[1];
guids[0] = new Guid("AB312E34-8025-40F2-8241-1958793F3D39");
}
private void CreateElement(IList <string> lines, string line, IDictionary <string, IJoint> joints)
{
string[] parts = line.Split(' ');
IElement element = elementFactory.Create(PopLine(lines));
AddElementPoints(element, PopLine(lines));
AddElementColors(element, PopLine(lines));
element.ZIndex = Convert.ToSingle(PopLine(lines));
string parentName = GetParentName(parts[1]);
IJoint parentJoint = joints[parentName];
parentJoint.AddElement(element);
}
void Add(IJoint joint)
{
if (joint is FixedJoint fixedJoint)
{
FixedJointAdapter jointAdapter = new FixedJointAdapter(fixedJoint);
_jointAdapters.Add(jointAdapter);
}
else if (joint is HingeJoint hingeJoint)
{
HingeJointAdapter jointAdapter = new HingeJointAdapter(hingeJoint);
_jointAdapters.Add(jointAdapter);
}
}
private void MouseScroll(IBody body, IJoint rightHand, IJoint spineMid)
{
var zRightHandBodyDistance = spineMid.Position.Z - rightHand.Position.Z;
if (zRightHandBodyDistance > 0.5f)
{
if (body.HandRightState == HandState.Closed)
{
if (startScrollGesture == null)
{
startScrollGesture = new MouseGestureState(PHelper.CurrentTimeMillis());
startScrollGesture.oldCursorPosition = Cursor.Position;
mouseInScroll = true;
}
if (startScrollClickGesture == null)
{
startScrollClickGesture = new MouseGestureState(PHelper.CurrentTimeMillis());
startScrollClickGesture.oldCursorPosition = Cursor.Position;
}
else
{
var currentTime = PHelper.CurrentTimeMillis();
var timeDiff = currentTime - startScrollClickGesture.TimeStamp;
if (timeDiff > 1000)
{
MouseControl.ScrollClick();
startScrollGesture = null;
startScrollClickGesture = null;
mouseInScroll = false;
}
}
}
else if (body.HandRightState == HandState.Open)
{
if (startScrollGesture != null)
{
var currentPosition = Cursor.Position;
var yMovement = -1 * (currentPosition.Y - startScrollGesture.oldCursorPosition.Y);
var times = 10;
MouseControl.Scroll(times);
startScrollGesture = null;
mouseInScroll = false;
}
}
else
{
//ignore
}
}
}
//Constructor
public BVHJoint(int id, string name, IJoint parent)
{
FId = id;
FName = name;
FParent = parent;
FChildren = new List <IJoint>();
FBaseTransform = VMath.IdentityMatrix;
FAnimationTransform = VMath.IdentityMatrix;
FConstraints = new List <Vector2D>();
FConstraints.Add(new Vector2D(-1.0, 1.0));
FConstraints.Add(new Vector2D(-1.0, 1.0));
FConstraints.Add(new Vector2D(-1.0, 1.0));
}
private void CreateJoint(IList <string> lines, string line, IDictionary <string, IJoint> joints, IDictionary <string, string> structures)
{
string[] parts = line.Split(' ');
if (structures.ContainsKey(parts[2]))
{
IJoint joint = CreateFromStructure(structures[parts[2]], null);
CommonCreateJoint(lines, joints, parts, joint);
}
else
{
throw new ArgumentException(string.Format("Dynamic joint \"{0}\" references unknown sub-structure \"{1}\".", parts[1], parts[2]));
}
}
public static IJoint Div(IJoint j, int divisor)
{
var newJoint = new OrientedJoint(j.JointType, j.Valid)
{
Point = j.Point/divisor,
Orientation = j.Orientation/divisor
};
foreach (var child in j.GetChildren())
{
newJoint.AddChild(Div(child, divisor));
}
return newJoint;
}
//Constructor
public BVHJoint(int id, string name, IJoint parent)
{
FId = id;
FName = name;
FParent = parent;
FChildren = new List<IJoint>();
FBaseTransform = VMath.IdentityMatrix;
FAnimationTransform = VMath.IdentityMatrix;
FConstraints = new List<Vector2D>();
FConstraints.Add(new Vector2D(-1.0, 1.0));
FConstraints.Add(new Vector2D(-1.0, 1.0));
FConstraints.Add(new Vector2D(-1.0, 1.0));
}
public bool Equals(IJoint o)
{
foreach (var child in children)
{
var oc = o.FindChild(child.Value.JointType);
if (!oc.Equals(child.Value))
{
return(false);
}
}
return(isValid == o.Valid && type == o.JointType &&
orientation.Equals(o.Orientation) && point.Equals(o.Point));
}
private void initRotations()
{
rotations = new Dictionary <string, Vector3D>();
jointChain = new List <IJoint>();
IJoint currJoint = outputSkeleton.JointTable[chainEnd];
while (currJoint.Name != chainStart)
{
currJoint = currJoint.Parent;
rotations.Add(currJoint.Name, new Vector3D(0));
jointChain.Add(currJoint);
}
jointChain.Reverse();
}
public static IJoint Add(IJoint j1, IJoint j2)
{
var newJoint = new OrientedJoint(j1.JointType, j1.Valid)
{
Point = j1.Point + j2.Point,
Orientation = j1.Orientation + j2.Orientation
};
foreach (var child in j1.GetChildren())
{
newJoint.AddChild(Add(child, j2.FindChild(child.JointType)));
}
return newJoint;
}
public static IJoint Add(IJoint j1, IJoint j2)
{
var newJoint = new OrientedJoint(j1.JointType, j1.Valid)
{
Point = j1.Point + j2.Point,
Orientation = j1.Orientation + j2.Orientation
};
foreach (var child in j1.GetChildren())
{
newJoint.AddChild(Add(child, j2.FindChild(child.JointType)));
}
return(newJoint);
}
private BodyPart ExtractData(IJoint joint)
{
var rotation = joint.Orientation;
var worldCoordinate = joint.Point;
var part = new BodyPart
{
Id = joint.JointType.ToInt(),
Position = worldCoordinate,
Rotation = rotation,
Length = Creator.GetDefaultBoneLength(joint.JointType),
Velocity = new Vector3(),
Acceleration = new Vector3(),
AngularVelocity = new Vector3(),
AngularAcceleration = new Vector3()
};
return part;
}
/// <summary>
///
/// </summary>
/// <param name="j1"></param>
/// <param name="j2"></param>
/// <returns></returns>
public static IJoint Diff(IJoint j1, IJoint j2)
{
if (j1.JointType != j2.JointType)
{
throw new Exception("Joint types defer from each other.");
}
var newJoint = new OrientedJoint(j1.JointType, j1.Valid)
{
Point = j1.Point - j2.Point,
Orientation = j1.Orientation - j2.Orientation
};
foreach (var child in j1.GetChildren())
{
newJoint.AddChild(Diff(child, j2.FindChild(child.JointType)));
}
return newJoint;
}
public bool IdentifyCloseHandAfterTime(IJoint rightHand, IJoint leftHand, IJoint spine, IJoint shoulderCenter, bool isRightHandGripped, bool isLeftHandGripped)
{
float betweenSpineAndShoulderHeight = spine.Position.Y + (shoulderCenter.Position.Y - spine.Position.Y)/2;
if (rightHand.Position.Y < betweenSpineAndShoulderHeight || GestureUtils.CalculateDistanceY(rightHand.Position, leftHand.Position) < 0.2
|| isLeftHandGripped)
{
State = 0;
return false;
}
//Keep track of initial position
if (State == 0 && !isRightHandGripped)
{
initialHandPosition = rightHand.Position;
closeHandIntervalControl.TriggerIt();
State = 1;
return false;
}
if (State == 1)
{
lastHandPosition = rightHand.Position;
//If hand moved, reset state
if (GestureUtils.CalculateDistanceY(initialHandPosition, lastHandPosition) > 0.05 ||
GestureUtils.CalculateDistanceX(initialHandPosition, lastHandPosition) > 0.05)
{
State = 0;
return false;
}
if( closeHandIntervalControl.HasIntervalPassed() && isRightHandGripped)
{
State = 2;
return true;
}
}
return false;
}
public static bool IsUserFacingForward(IJoint shoulderRight, IJoint shoulderLeft, IJoint center)
{
float angleOfUserCenterTillKinectDevice = GetAngleOfLineBetweenTwoPoints(new SkeletonPoint(), center.Position);
float angleOfUserShoulderLine = GetAngleOfLineBetweenTwoPoints(shoulderLeft.Position, shoulderRight.Position);
float angleDifference = angleOfUserCenterTillKinectDevice - angleOfUserShoulderLine;
angleDifference -= 90f;
if (Math.Abs(angleDifference) > 25)
{
return false;
}
else
{
return true;
}
}
public static bool IsOneHandBelowSpine(IJoint rightHand, IJoint leftHand, IJoint spine)
{
return rightHand.Position.Y < spine.Position.Y || leftHand.Position.Y < spine.Position.Y;
}
public static bool IsHandBelow(IJoint rightHand, IJoint otherJoint)
{
return rightHand.Position.Y < otherJoint.Position.Y;
}
public static bool IsHandBelow(IJoint rightHand, SkeletonPoint aPoint, float extra)
{
return rightHand.Position.Y < aPoint.Y - extra;
}
public bool IdentifyRotationGesture(IJoint rightHand, IJoint leftHand, IJoint spine, bool isLeftHandGripped, bool isRightHandGripped)
{
if (
GestureUtils.AreHandsBelowSpine(rightHand,leftHand,spine)
)
{
State = 0;
CurrentAngleDelta = 0;
return false;
}
var handInFront = ( rightHand.Position.Z > leftHand.Position.Z ) ? rightHand : leftHand;
float halfDistanceBetweenHandAndSpine = GestureUtils.HalfDistanceBetweenHandAndSpine(handInFront, spine);
if(GestureUtils.AreHandsSeparatedInZ(rightHand,leftHand, halfDistanceBetweenHandAndSpine))
{
State = 0;
CurrentAngleDelta = 0;
return false;
}
if(State == 0)
{
if(!GestureUtils.AreHandsSeparatedInY(rightHand,leftHand, MinimumYDistanceToStartRotation) )
{
return false;
}
}
if (
isLeftHandGripped && isRightHandGripped && State == 0
)
{
initialRightHand = rightHand.Position;
initialLeftHand = leftHand.Position;
initialAngle = GestureUtils.AngleBetweenPoints(initialLeftHand, initialRightHand);
CurrentAngleDelta = 0;
State = 1;
Output.Debug("RotationGesture","Initial position");
return false;
}
else if ( !isLeftHandGripped || !isRightHandGripped)
{
State = 0;
CurrentAngleDelta = 0;
return false;
}
if (State == 1)
{
bool rightWasUp = (initialRightHand.Y > initialLeftHand.Y) ? true : false;
bool isRightUpNow = (rightHand.Position.Y > leftHand.Position.Y) ? true : false;
float currentAngle = GestureUtils.AngleBetweenPoints(leftHand.Position, rightHand.Position);
CurrentAngleDelta = initialAngle - currentAngle;
//If hands were significantly distant and changed heights then rotation happened
if (rightWasUp != isRightUpNow)
{
Output.Debug("RotationGesture", "One hand above other. Right hand was up ? " + rightWasUp);
if (rightWasUp)
{
Name = GestureEvents.ROTATE_RIGHT;
State = 2;
return true;
}
else
{
Name = GestureEvents.ROTATE_LEFT;
State = 2;
return true;
}
}
else
{
if (Math.Abs(CurrentAngleDelta) > MinimumAngleChangeToTriggerRotation)
{
if (rightWasUp)
{
if (CurrentAngleDelta > 0)
{
Name = GestureEvents.ROTATE_RIGHT;
State = 2;
return true;
}
else
{
Name = GestureEvents.ROTATE_LEFT;
State = 2;
return true;
}
}
}
}
return false;
}
return false;
}
public void Update(JointType jt, IJoint j)
{
if (FindChild(jt).JointType != jt)
{
var sorted = children.Keys.OrderByDescending(x => x);
try
{
var key = sorted.First(x => x < jt);
children[key].Update(jt, j);
}
catch (InvalidOperationException)
{ }
}
else
{
RemoveChild(jt);
AddChild(j);
}
}
public bool Equals(IJoint o)
{
foreach(var child in children) {
var oc = o.FindChild(child.Value.JointType);
if (!oc.Equals(child.Value))
{
return false;
}
}
return isValid == o.Valid && type == o.JointType
&& orientation.Equals(o.Orientation) && point.Equals(o.Point);
}
public IJoint Append(IJoint j)
{
AddChild(j);
return j;
}
private bool Filter(IJoint joint)
{
var allowedJoints = new List<JointType> { JointType.ELBOW_LEFT, JointType.ELBOW_RIGHT, JointType.WRIST_LEFT, JointType.WRIST_RIGHT, JointType.HAND_LEFT, JointType.HAND_RIGHT };
return allowedJoints.Contains(joint.JointType);
}
public static bool IsLeftHandCloseToRightOrInFront(IJoint rightHand, IJoint leftHand, float minimumDistanceAllowed)
{
//Less Z is closer to sensor
return leftHand.Position.Z < rightHand.Position.Z || leftHand.Position.Z < rightHand.Position.Z + minimumDistanceAllowed;
}
public static float HalfDistanceBetweenHandAndSpine(IJoint rightHand, IJoint spine)
{
float distance = (rightHand.Position.Z - spine.Position.Z) / 2;
return Math.Abs(distance);
}
/// <summary>
/// Update (and filter) the joint position based on the referenced <c>IJoint</c>.
/// </summary>
public override void Update(IJoint joint)
{
_trackingState = joint.TrackingState;
if (_trackingState == TrackingState.NotTracked)
{
_position = new CameraSpacePoint();
Reset();
return;
}
// If not tracked, we smooth a bit more by using a bigger jitter radius
// Always filter feet highly as they are so noisy
var jitterRadius = _jitterRadius;
var maxDeviationRadius = _maxDeviationRadius;
if (joint.TrackingState != TrackingState.Tracked)
{
jitterRadius *= 2.0f;
maxDeviationRadius *= 2.0f;
}
Vector3 filteredPosition,
positionDelta,
trend;
float diffVal;
var reportedPosition = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
var prevFilteredPosition = _history.FilteredPosition;
var prevTrend = _history.Trend;
var prevRawPosition = _history.ReportedPosition;
// If joint is invalid, reset the filter
if (reportedPosition.X == 0 && reportedPosition.Y == 0 && reportedPosition.Z == 0)
{
Reset();
}
// Initial start values
if (this._history.FrameCount == 0)
{
filteredPosition = reportedPosition;
trend = new Vector3();
}
else if (this._history.FrameCount == 1)
{
filteredPosition = (reportedPosition + prevRawPosition) * 0.5f;
positionDelta = filteredPosition - prevFilteredPosition;
trend = (positionDelta * _correction) + (prevTrend * (1.0f - _correction));
}
else
{
// First apply jitter filter
positionDelta = reportedPosition - prevFilteredPosition;
diffVal = Math.Abs(positionDelta.Length());
if (diffVal <= jitterRadius)
{
filteredPosition = (reportedPosition * (diffVal / jitterRadius)) +
(prevFilteredPosition * (1.0f - (diffVal / jitterRadius)));
}
else
{
filteredPosition = reportedPosition;
}
// Now the double exponential smoothing filter
filteredPosition = (filteredPosition * (1.0f - _smoothing)) +
((prevFilteredPosition + prevTrend) * _smoothing);
positionDelta = filteredPosition - prevFilteredPosition;
trend = (positionDelta * _correction) + (prevTrend * (1.0f - _correction));
}
// Predict into the future to reduce latency
var predictedPosition = filteredPosition + (trend * _prediction);
// Check that we are not too far away from raw data
positionDelta = predictedPosition - reportedPosition;
diffVal = Math.Abs(positionDelta.Length());
if (diffVal > maxDeviationRadius)
{
predictedPosition = (predictedPosition * (maxDeviationRadius / diffVal)) +
(reportedPosition * (1.0f - (maxDeviationRadius / diffVal)));
}
// Save the data from this frame
this._history.ReportedPosition = reportedPosition;
this._history.FilteredPosition = filteredPosition;
this._history.Trend = trend;
this._history.FrameCount++;
// Set the filtered data back into the joint
_position.X = predictedPosition.X;
_position.Y = predictedPosition.Y;
_position.Z = predictedPosition.Z;
_depthPosition = new DepthSpacePoint();
_colorPosition = new ColorSpacePoint();
}
public static bool IsHandCloseToSpineInZ(IJoint hand, IJoint spine, float minimumDistance)
{
return Math.Abs(hand.Position.Z - spine.Position.Z) < minimumDistance;
}
/// <summary>
/// Update the joint position based on the referenced <c>IJoint</c>.
/// </summary>
/// <param name="joint"></param>
/// <exception cref="System.NotSupportedException">Must update a KinectJoint with a native Kinect SDK Joint</exception>
public void Update(IJoint joint)
{
throw new NotSupportedException("Must update a KinectJoint with a native Kinect SDK Joint");
}
public static bool AreHandsSeparatedInY(IJoint rightHand, IJoint leftHand, float distance)
{
return Math.Abs(GestureUtils.CalculateDistanceY(rightHand.Position, leftHand.Position)) > distance;
}
public static bool AreHandsBelowSpine(IJoint rightHand, IJoint leftHand, IJoint spine)
{
return rightHand.Position.Y < spine.Position.Y && leftHand.Position.Y < spine.Position.Y;
}
private static DepthSpacePoint GetDepthSpacePoint(IJoint joint, bool hasMappedDepthPositions)
{
if (hasMappedDepthPositions)
{
return joint.DepthPosition;
}
else
{
return new DepthSpacePoint()
{
X = (joint.Position.X * _halfWidth) + _halfWidth,
Y = (joint.Position.Y * -_halfHeight) + _halfHeight
};
}
}