// Use this for initialization
void Awake()
{
m_transform = transform;
bool beckonAlive = BeckonManager.BeckonInstance.IsInit();
if (beckonAlive)
{
OMKStatus rc;
m_currentSkeleton = Factory.createSkeleton(out rc);
if (rc == OMKStatus.OMK_SUCCESS)
{
InitializeHeirarchy();
CheckForPersonChange();
}
else
{
Debug.LogError("Couldn't create Skeleton: " + rc, this);
}
}
else
{
Debug.LogError("Beckon is not initialized");
}
}
public static void CopyData(this ISkeleton src, ISkeleton dst)
{
if (src == null || dst == null)
{
return;
}
if (src.JointTable.Count != dst.JointTable.Count || src.Uid != dst.Uid)
{
dst.ClearAll();
dst.Root = src.Root?.DeepCopy() ?? dst.Root;
dst.BuildJointTable();
dst.Uid = src.Uid;
}
if (src.JointTable != null)
{
foreach (var joint in src.JointTable.Keys)
{
if (!dst.JointTable.ContainsKey(joint))
{
continue;
}
dst.JointTable[joint].BaseTransform = src.JointTable[joint].BaseTransform;
dst.JointTable[joint].AnimationTransform = src.JointTable[joint].AnimationTransform;
}
}
}
public void CheckAll(ISkeleton skeleton, ISkeletonReport report)
{
foreach (ISkeletonConstraint constraint in constraints)
{
constraint.Check(skeleton, report);
}
}
/// <summary>
/// Initializes a new instance of the <see cref="Trame"/> class.
/// </summary>
/// <param name="dt">Dt.</param>
public Trame(DeviceType dt)
{
last = Creator.GetNewInvalidSkeleton();
currentType = dt;
resetTimer = new Timer(1000);
resetTimer.AutoReset = true;
lastUpdate = DateTime.Now;
resetTimer.Elapsed += (sender, args) =>
{
if ((dt == DeviceType.KINECT || dt == DeviceType.LEAP_MOTION_AND_KINECT) && !resetInProgress && (DateTime.Now - lastUpdate).TotalMilliseconds > 2000)
{
resetInProgress = true;
// reset trame
UpdatedType();
currentDevice.Start();
resetInProgress = false;
lastUpdate = DateTime.Now;
}
};
UpdatedType();
}
/// <summary>
///
/// </summary>
/// <param name="s1"></param>
/// <param name="s2"></param>
/// <returns></returns>
public static SkeletonDiff Diff(ISkeleton s1, ISkeleton s2)
{
var skelDiff = new SkeletonDiff {
Root = SkeletonDiff.Diff(s1.Root, s2.Root)
};
return(skelDiff);
}
public SkeletalAnimationEngine(ISkeleton target)
{
_target = target;
if (_target is ISerializable) { ID = string.Concat(((ISerializable)_target).ID, "-skeletal-animation-engine"); }
_boneFrameStartValues = new Dictionary<string, SkeletalKeyframe.DataContainer>();
_boneFrameEndValues = new Dictionary<string, SkeletalKeyframe.DataContainer>();
}
public static ISkeleton Add(ISkeleton s, SkeletonDiff diff)
{
var newSkeleton = new Skeleton {
Root = SkeletonDiff.Add(s.Root, diff.Root)
};
return(newSkeleton);
}
public SkeletonExtension(ISkeleton extendingFrame, ISkeleton baseFrame)
{
this.extendingFrame = extendingFrame;
this.baseFrame = baseFrame;
if (this.baseFrame == null)
{
this.baseFrame = this;
}
}
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 SkeletalFrame(ISkeleton baseFrame)
{
this.baseFrame = baseFrame;
if (baseFrame == null)
{
this.baseFrame = this;
}
this.IsLoaded = false;
this.Bones = null;
}
public SkeletalAnimationEngine(ISkeleton target)
{
_target = target;
if (_target is ISerializable)
{
ID = string.Concat(((ISerializable)_target).ID, "-skeletal-animation-engine");
}
_boneFrameStartValues = new Dictionary <string, SkeletalKeyframe.DataContainer>();
_boneFrameEndValues = new Dictionary <string, SkeletalKeyframe.DataContainer>();
}
public void CalculateJointMatrix(ISkeleton baseFrame)
{
if (this.JointMatrixSet) { return; }
this.JointMatrix = Matrix.Scaling(this.Scale);
this.JointMatrix *= Matrix.RotationQuaternion(this.Orientation);
this.JointMatrix *= Matrix.Translation(this.Position);
this.JointMatrix = baseFrame.GetBoneById(this.Id).InverseJointMatrix * this.JointMatrix;
this.JointMatrixSet = true;
}
public AnimatedSkeleton(ISkeleton baseFrame, MtbSection mtbSection)
{
if (baseFrame == null) { throw new ArgumentNullException("baseFrame"); }
this.baseFrame = baseFrame;
this.IsLoaded = false;
this.Bones = null;
this.mtbSection = mtbSection;
this.Fps = mtbSection.Header.Float1;
this.TotalFrames = mtbSection.Header.AnimationLength;
this.AnimationLength = this.TotalFrames / this.Fps;
}
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");
}
public void CalculateJointMatrix(ISkeleton baseFrame)
{
if (this.JointMatrixSet)
{
return;
}
this.JointMatrix = Matrix.Scaling(this.Scale);
this.JointMatrix *= Matrix.RotationQuaternion(this.Orientation);
this.JointMatrix *= Matrix.Translation(this.Position);
this.JointMatrix = baseFrame.GetBoneById(this.Id).InverseJointMatrix *this.JointMatrix;
this.JointMatrixSet = true;
}
public AnimatedSkeleton(ISkeleton baseFrame, MtbSection mtbSection)
{
if (baseFrame == null)
{
throw new ArgumentNullException("baseFrame");
}
this.baseFrame = baseFrame;
this.IsLoaded = false;
this.Bones = null;
this.mtbSection = mtbSection;
this.Fps = mtbSection.Header.Float1;
this.TotalFrames = mtbSection.Header.AnimationLength;
this.AnimationLength = this.TotalFrames / this.Fps;
}
internal static void SetObjects(Dictionary <string, object> objects)
{
EngineApplicationInterface._objects = objects;
EngineApplicationInterface.IPath = EngineApplicationInterface.GetObject <IPath>();
EngineApplicationInterface.IShader = EngineApplicationInterface.GetObject <IShader>();
EngineApplicationInterface.ITexture = EngineApplicationInterface.GetObject <ITexture>();
EngineApplicationInterface.IMaterial = EngineApplicationInterface.GetObject <IMaterial>();
EngineApplicationInterface.IMetaMesh = EngineApplicationInterface.GetObject <IMetaMesh>();
EngineApplicationInterface.IDecal = EngineApplicationInterface.GetObject <IDecal>();
EngineApplicationInterface.IClothSimulatorComponent = EngineApplicationInterface.GetObject <IClothSimulatorComponent>();
EngineApplicationInterface.ICompositeComponent = EngineApplicationInterface.GetObject <ICompositeComponent>();
EngineApplicationInterface.IPhysicsShape = EngineApplicationInterface.GetObject <IPhysicsShape>();
EngineApplicationInterface.IBodyPart = EngineApplicationInterface.GetObject <IBodyPart>();
EngineApplicationInterface.IMesh = EngineApplicationInterface.GetObject <IMesh>();
EngineApplicationInterface.IMeshBuilder = EngineApplicationInterface.GetObject <IMeshBuilder>();
EngineApplicationInterface.ICamera = EngineApplicationInterface.GetObject <ICamera>();
EngineApplicationInterface.ISkeleton = EngineApplicationInterface.GetObject <ISkeleton>();
EngineApplicationInterface.IGameEntity = EngineApplicationInterface.GetObject <IGameEntity>();
EngineApplicationInterface.IGameEntityComponent = EngineApplicationInterface.GetObject <IGameEntityComponent>();
EngineApplicationInterface.IScene = EngineApplicationInterface.GetObject <IScene>();
EngineApplicationInterface.IScriptComponent = EngineApplicationInterface.GetObject <IScriptComponent>();
EngineApplicationInterface.ILight = EngineApplicationInterface.GetObject <ILight>();
EngineApplicationInterface.IParticleSystem = EngineApplicationInterface.GetObject <IParticleSystem>();
EngineApplicationInterface.IPhysicsMaterial = EngineApplicationInterface.GetObject <IPhysicsMaterial>();
EngineApplicationInterface.ISceneView = EngineApplicationInterface.GetObject <ISceneView>();
EngineApplicationInterface.IView = EngineApplicationInterface.GetObject <IView>();
EngineApplicationInterface.ITableauView = EngineApplicationInterface.GetObject <ITableauView>();
EngineApplicationInterface.ITextureView = EngineApplicationInterface.GetObject <ITextureView>();
EngineApplicationInterface.IVideoPlayerView = EngineApplicationInterface.GetObject <IVideoPlayerView>();
EngineApplicationInterface.IThumbnailCreatorView = EngineApplicationInterface.GetObject <IThumbnailCreatorView>();
EngineApplicationInterface.IDebug = EngineApplicationInterface.GetObject <IDebug>();
EngineApplicationInterface.ITwoDimensionView = EngineApplicationInterface.GetObject <ITwoDimensionView>();
EngineApplicationInterface.IUtil = EngineApplicationInterface.GetObject <IUtil>();
EngineApplicationInterface.IEngineSizeChecker = EngineApplicationInterface.GetObject <IEngineSizeChecker>();
EngineApplicationInterface.IInput = EngineApplicationInterface.GetObject <IInput>();
EngineApplicationInterface.ITime = EngineApplicationInterface.GetObject <ITime>();
EngineApplicationInterface.IScreen = EngineApplicationInterface.GetObject <IScreen>();
EngineApplicationInterface.IMusic = EngineApplicationInterface.GetObject <IMusic>();
EngineApplicationInterface.IImgui = EngineApplicationInterface.GetObject <IImgui>();
EngineApplicationInterface.IMouseManager = EngineApplicationInterface.GetObject <IMouseManager>();
EngineApplicationInterface.IHighlights = EngineApplicationInterface.GetObject <IHighlights>();
EngineApplicationInterface.ISoundEvent = EngineApplicationInterface.GetObject <ISoundEvent>();
EngineApplicationInterface.ISoundManager = EngineApplicationInterface.GetObject <ISoundManager>();
EngineApplicationInterface.IConfig = EngineApplicationInterface.GetObject <IConfig>();
EngineApplicationInterface.IManagedMeshEditOperations = EngineApplicationInterface.GetObject <IManagedMeshEditOperations>();
}
public void ProcessSkeleton(BoneMarkers marker, ISkeleton skeleton)
{
SkeletonProcessRequest p = new SkeletonProcessRequest();
p.Analyzer = _analyzer as ISkeletonAnalyzer;
p.CurrentMarker = marker;
p.CurrentSkeleton = skeleton;
lock (_lockObject)
{
_requestQueue.Enqueue(p);
if (_requestQueue.Count == 1) // the only element that we just inserted is there
{
Monitor.Pulse(_lockObject);
}
}
}
public void SetMetricsAndCreateChildrenFromXml(ISkeleton skeleton, XElement boneData)
{
_relativePosition = new Vector2((float)boneData.Attribute("offset-x"), (float)boneData.Attribute("offset-y"));
_relativeRotation = boneData.Attribute("rotation") == null ? 0.0f : MathHelper.ToRadians((float)boneData.Attribute("rotation"));
_relativeScale = boneData.Attribute("scale") == null ? 1.0f : (float)boneData.Attribute("scale");
_relativeDepth = (float)boneData.Attribute("depth");
_relativeTint = Color.White;
if (boneData.Elements("bone") != null)
{
foreach (XElement bone in boneData.Elements("bone"))
{
StorableBone newBone = new StorableBone();
newBone.ID = bone.Attribute("id").Value;
skeleton.AddBone(newBone, boneData.Attribute("id").Value);
newBone.SetMetricsAndCreateChildrenFromXml(skeleton, bone);
}
}
}
private void UpdateSkeletons()
{
if (_bodyDataBuffer.Length > 0)
{
var bodies = _bodyDataBuffer.Where(body => body.IsTracked);
foreach (KinectV2::Microsoft.Kinect.Body body in bodies)
{
var knownSkeleton = _knownSkeletons.FirstOrDefault(skeleton => skeleton.ID == (uint)body.TrackingId);
if (knownSkeleton != null)
{
UpdateSkeleton(body, knownSkeleton);
}
else
{
_lastAddedSkeleton = CreateSkeleton(body);
_knownSkeletons.Add(_lastAddedSkeleton);
NewSkeleton?.Invoke(_lastAddedSkeleton);
}
}
}
}
public void OnSkeletonChanged(BoneMarkers marker, ISkeleton newSkeleton, IGame game)
{
if (_writer == null)
{
return;
}
for (int i = 0; i < (int)SkeletonMarkers.Count - 1; i++)
{
Vector3 v = newSkeleton.GetPositionOf((SkeletonMarkers)i);
_currentData[i] = new Vub.Etro.IO.Vector4(v.X, v.Y, v.Z, 0);
}
writeGameObjects(game, (int)labels.Length);
//if (newSkeleton is IRichSkeleton)
//{
// writeSkeletonAngles((IRichSkeleton)newSkeleton, labels.Length + game.GameObjects.Keys.Count);
// writeSkeletonQualities((IRichSkeleton)newSkeleton, labels.Length + game.GameObjects.Keys.Count + angleLabels.Length);
//}
_writer.WriteIntFrame(_currentData);
WriteAnalogData(game);
}
/// <summary>
/// Initialize tracking system. Called once on game startup.
/// </summary>
void Start()
{
av = new MovingAverage(averagingSamples, logTrackingData);
disList.Add(av);
var logPath = Path.Combine(Application.dataPath, "Logs");
// Create Logger
nLog = new NamedLogger(
new Dictionary <string, string> {
{ "rAngle", "rAngle" },
{ "lAngle", "lAngle" },
{ "rElev", "rElev" },
{ "rHorizX", "rHorizX" },
{ "rHorizZ", "rHorizZ" }
},
logTrackingData, logPath);
// Add logger to list of objects to dispose of
disList.Add(nLog);
sk = new Skelly(maxJointVelocity, true);
}
public ISkeleton GetSkeleton(ISkeleton s)
{
var result = _lastSkeleton;
_lastSkeleton = s;
return result;
}
/// <summary>
/// Fires the new skeleton.
/// </summary>
/// <param name="skeleton">Skeleton.</param>
private void FireNewSkeleton(ISkeleton skeleton)
{
last = skeleton;
lastUpdate = DateTime.Now;
NewSkeleton?.Invoke(skeleton);
}
public static ISkeleton Add(ISkeleton s, SkeletonDiff diff)
{
var newSkeleton = new Skeleton {Root = SkeletonDiff.Add(s.Root, diff.Root)};
return newSkeleton;
}
/// <summary>
/// Determines whether the specified <see cref="ISkeleton"/> is equal to the current <see cref="Skeleton"/>.
/// </summary>
/// <param name="other">The <see cref="ISkeleton"/> to compare with the current <see cref="Skeleton"/>.</param>
/// <returns><c>true</c> if the specified <see cref="ISkeleton"/> is equal to the current
/// <see cref="Skeleton"/>; otherwise, <c>false</c>.</returns>
public bool Equals(ISkeleton other)
{
return(valid == other.Valid && root.Equals(other.Root));
}
public void Check(ISkeleton skeleton, ISkeletonReport report)
{
if (Wishness == 0)
{
return;
}
//the positions we recieve from the Kinect are absolute positions ...
//we have to use relative position if we want to apply a constraint position efficiently
//-> use the hipcenter as (0,0,0) and convert the rest in relative positions
//warning : the values are in meter !
Vector3 currentRelativePosition;
#region getCurrentRelativePosition
switch (Joint)
{
case PlayerJoint.HipCenter:
currentRelativePosition = Vector3.Zero;
break;
case PlayerJoint.AnkleLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.AnkleLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.AnkleRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.AnkleRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.ElbowLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.ElbowRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.FootLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.FootLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.FootRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.FootRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.HandLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HandLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.HandRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HandRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.Head:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.Head),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.HipLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.HipRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.KneeLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.KneeRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.Sternum:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.Sternum),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.ShoulderCenter:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.ShoulderLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.ShoulderRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.Spine:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.Spine),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.WristLeft:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.WristLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
case PlayerJoint.WristRight:
currentRelativePosition = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.WristRight),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));
break;
default:
currentRelativePosition = Vector3.Zero;
break;
}
#endregion
//distance recieved in millimeters????
currentRelativePosition /= 1000;
//if the values are the same we do not test this direction
bool xProblemAnalyze = MinPos.X != MaxPos.X;
bool yProblemAnalyze = MinPos.Y != MaxPos.Y;
bool zProblemAnalyze = MinPos.Z != MaxPos.Z;
if (Wishness > 0)
{ //we want to reach this position
if ((xProblemAnalyze &&
(currentRelativePosition.X < MinPos.X || currentRelativePosition.X > MaxPos.X)) ||
(yProblemAnalyze &&
(currentRelativePosition.Y < MinPos.Y || currentRelativePosition.Y > MaxPos.Y)) ||
(zProblemAnalyze &&
(currentRelativePosition.Z < MinPos.Z || currentRelativePosition.Z > MaxPos.Z)))
{//well there's one problem but we don't really care which one here
if (report.GeneralSeverity.CompareTo(Math.Abs(Wishness)) < 0)
{
report.GeneralSeverity = Math.Abs(Wishness);
}
report.CorrectiveCollection.CorrectiveItems.Add(new SkeletonCorrecticePosition(
Joint, currentRelativePosition,
Vector3.Divide(Vector3.Add(MinPos, MaxPos), 2),
MinPos, MaxPos, Wishness));
}
}
else if (Wishness < 0)
{ //we don't want to reach this position
bool xProblemDetected = false;
bool yProblemDetected = false;
bool zProblemDetected = false;
if (xProblemAnalyze &&
currentRelativePosition.X > MinPos.X &&
currentRelativePosition.X < MaxPos.X)
{//problem and we want to check it
xProblemDetected = true;
}
if (yProblemAnalyze &&
currentRelativePosition.Y > MinPos.Y &&
currentRelativePosition.Y < MaxPos.Y)
{
yProblemDetected = true;
}
if (zProblemAnalyze &&
currentRelativePosition.Z > MinPos.Z &&
currentRelativePosition.Z < MaxPos.Z)
{
zProblemAnalyze = true;
}
if ((xProblemDetected || !xProblemAnalyze) &&
(yProblemDetected || !yProblemAnalyze) &&
(zProblemDetected || !zProblemAnalyze) &&
(xProblemAnalyze || yProblemAnalyze || zProblemAnalyze))
{//here we can be sure that, if we want to detect a problem on one or mutiple direction, it will be detected
//we are in a position which is not good !!
if (report.GeneralSeverity.CompareTo(Math.Abs(Wishness)) < 0)
{
report.GeneralSeverity = Math.Abs(Wishness);
}
report.CorrectiveCollection.CorrectiveItems.Add(new SkeletonCorrecticePosition(
Joint, currentRelativePosition,
Vector3.Divide(Vector3.Add(MinPos, MaxPos), 2),
MinPos, MaxPos, Wishness));
}
}
}
public ISkeleton GetSkeleton(ISkeleton baseSkeleton)
{
return baseSkeleton;
}
/// <summary>
/// Add new data to the data stream
/// </summary>
/// <param name="skeleton">a new skeleton</param>
public void PushNewSkeleton(ISkeleton skeleton)
{
_skeletonBuffer.Add(skeleton.Clone());
_skeletonBuffer2.Add(skeleton.Clone());
}
public SkeletonRenderer(ISkeleton skeleton,
BoneTransformManager boneTransformManager)
{
this.Skeleton = skeleton;
this.boneTransformManager_ = boneTransformManager;
}
/// <summary>
/// Determines whether the specified <see cref="Trame.ISkeleton"/> is equal to the current <see cref="Trame.Implementation.Skeleton.Skeleton"/>.
/// </summary>
/// <param name="other">The <see cref="Trame.ISkeleton"/> to compare with the current <see cref="Trame.Implementation.Skeleton.Skeleton"/>.</param>
/// <returns><c>true</c> if the specified <see cref="Trame.ISkeleton"/> is equal to the current
/// <see cref="Trame.Implementation.Skeleton.Skeleton"/>; otherwise, <c>false</c>.</returns>
public bool Equals(ISkeleton other)
{
return valid == other.Valid && root.Equals(other.Root);
}
public static ISkeleton SkeletonSmoothing(ISkeleton s1, ISkeleton s2, int windowSize)
{
var mean = Div(Diff(s1, s2), windowSize);
return Add(s1, mean);
}
public void PushNewSkeleton(ISkeleton skeleton) {}
/// <summary>
/// Initializes a new instance of the <see cref="Trame.Implementation.Device.LeapMotion"/> class.
/// </summary>
public LeapMotion()
{
_lastSkeleton = Creator.GetNewDefaultSkeleton<InMapSkeleton>();
}
public VisualSkeleton(ISkeleton skeleton)
{
this.skeletonFrame = skeleton;
}
/// <summary>
/// Determines whether the specified <see cref="ISkeleton"/> is equal to the current <see cref="Trame.Implementation.Skeleton.Skeleton"/>.
/// </summary>
/// <param name="other">The <see cref="ISkeleton"/> to compare with the current <see cref="Trame.Implementation.Skeleton.Skeleton"/>.</param>
/// <returns><c>true</c> if the specified <see cref="ISkeleton"/> is equal to the current
/// <see cref="Trame.Implementation.Skeleton.Skeleton"/>; otherwise, <c>false</c>.</returns>
public bool Equals(ISkeleton other)
{
return valid == other.Valid && _joints.Values.Equals(other.Joints);
}
/// <summary>
///
/// </summary>
/// <param name="s1"></param>
/// <param name="s2"></param>
/// <returns></returns>
public static SkeletonDiff Diff(ISkeleton s1, ISkeleton s2)
{
var skelDiff = new SkeletonDiff {Root = SkeletonDiff.Diff(s1.Root, s2.Root)};
return skelDiff;
}
private void UpdateFrameNumber()
{
Video.SetFrame(SliderValue);
using (Image <Bgr, Byte> orig = Video.GetFrameImage())
using (Image <Gray, Byte> origGray = orig.Convert <Gray, Byte>())
using (Image <Gray, Byte> binary = origGray.ThresholdBinary(new Gray(ThresholdValue), new Gray(255)))
using (Image <Gray, Byte> subbed = BinaryBackground.AbsDiff(binary))
{
CvBlobs blobs = new CvBlobs();
BlobDetector.Detect(subbed, blobs);
CvBlob mouseBlob = null;
double maxArea = -1;
foreach (var blob in blobs.Values)
{
if (blob.Area > maxArea)
{
mouseBlob = blob;
maxArea = blob.Area;
}
}
//double gapDistance = GetBestGapDistance(rbsk);
double gapDistance = 50;
RBSK.Settings.GapDistance = gapDistance;
//PointF[] headPoints = ProcessFrame(orig, RBSK);
PointF center = mouseBlob.Centroid;
//LineSegment2DF[] targetPoints = null;
Point[] mouseContour = mouseBlob.GetContour();
orig.DrawPolyline(mouseContour, true, new Bgr(Color.Cyan));
Image1 = ImageService.ToBitmapSource(orig);
PointF[] result;
if (HeadPoints != null)
{
result = HeadPoints[SliderValue].HeadPoints;
}
else
{
double prob = 0;
RBSK headRbsk = MouseService.GetStandardMouseRules();
headRbsk.Settings.GapDistance = 65;
headRbsk.Settings.BinaryThreshold = 20;
List <List <PointF> > allKeyPoints = headRbsk.FindKeyPoints(mouseContour, headRbsk.Settings.NumberOfSlides, false);
result = headRbsk.FindPointsFromRules(allKeyPoints[0], binary, ref prob);
}
if (result != null)
{
using (Image <Bgr, Byte> test = orig.Clone())
{
foreach (var point in result)
{
test.Draw(new CircleF(point, 3), new Bgr(Color.Red), 3);
}
Image1 = ImageService.ToBitmapSource(test);
}
}
else
{
return;
}
RotatedRect rotatedRect = CvInvoke.MinAreaRect(mouseContour.Select(x => new PointF(x.X, x.Y)).ToArray());
//Console.WriteLine("Size: " + rotatedRect.Size);
ISkeleton skel = ModelResolver.Resolve <ISkeleton>();
Image <Gray, Byte> tempBinary = binary.Clone();
System.Drawing.Rectangle rect = mouseBlob.BoundingBox;
Image <Gray, Byte> binaryRoi = tempBinary.GetSubRect(rect);
using (Image <Bgr, Byte> displayImage = subbed.Convert <Bgr, Byte>())
using (Image <Gray, Byte> skelImage = skel.GetSkeleton(binaryRoi))
using (Image <Bgr, Byte> drawImage = orig.Clone())
using (Image <Bgr, Byte> tempImage2 = new Image <Bgr, byte>(drawImage.Size))
{
//-----------------------------------------
if (SkelImage != null)
{
SkelImage.Dispose();
}
SkelImage = skelImage.Clone();
//--------------------------------------------
tempImage2.SetValue(new Bgr(Color.Black));
ISpineFinding spineFinder = ModelResolver.Resolve <ISpineFinding>();
spineFinder.NumberOfCycles = 3;
spineFinder.NumberOfIterations = 1;
spineFinder.SkeletonImage = skelImage;
//spineFinder.RotatedRectangle = rotatedRect;
Image5 = ImageService.ToBitmapSource(skelImage);
const int delta = 20;
double smallestAngle = double.MaxValue;
Point tailPoint = Point.Empty;
for (int i = 0; i < mouseContour.Length; i++)
{
int leftDelta = i - delta;
int rightDelta = i + delta;
if (leftDelta < 0)
{
leftDelta += mouseContour.Length;
}
if (rightDelta >= mouseContour.Length)
{
rightDelta -= mouseContour.Length;
}
Point testPoint = mouseContour[i];
Point leftPoint = mouseContour[leftDelta];
Point rightPoint = mouseContour[rightDelta];
Vector v1 = new Vector(leftPoint.X - testPoint.X, leftPoint.Y - testPoint.Y);
Vector v2 = new Vector(rightPoint.X - testPoint.X, rightPoint.Y - testPoint.Y);
double angle = Math.Abs(Vector.AngleBetween(v1, v2));
if (angle < 30 && angle > 9)
{
if (angle < smallestAngle)
{
smallestAngle = angle;
tailPoint = testPoint;
}
}
}
PointF headCornerCorrect = new PointF(result[2].X - rect.X, result[2].Y - rect.Y);
PointF tailCornerCorrect = new PointF(tailPoint.X - rect.X, tailPoint.Y - rect.Y);
PointF[] spine = spineFinder.GenerateSpine(headCornerCorrect, tailCornerCorrect);
Point topCorner = mouseBlob.BoundingBox.Location;
PointF[] spineCornerCorrected = new PointF[spine.Length];
for (int i = 0; i < spine.Length; i++)
{
spineCornerCorrected[i] = new PointF(spine[i].X + topCorner.X, spine[i].Y + topCorner.Y);
}
ITailFinding tailFinding = ModelResolver.Resolve <ITailFinding>();
double rotatedWidth = rotatedRect.Size.Width < rotatedRect.Size.Height ? rotatedRect.Size.Width : rotatedRect.Size.Height;
List <Point> bodyPoints;
if (result != null)
{
double firstDist = result[2].DistanceSquared(spineCornerCorrected.First());
double lastDist = result[2].DistanceSquared(spineCornerCorrected.Last());
if (firstDist < lastDist)
{
spineCornerCorrected = spineCornerCorrected.Reverse().ToArray();
}
}
double waistLength;
double pelvicArea1, pelvicArea2;
tailFinding.FindTail(mouseContour, spineCornerCorrected, displayImage, rotatedWidth, mouseBlob.Centroid, out bodyPoints, out waistLength, out pelvicArea1, out pelvicArea2);
Console.WriteLine(smallestAngle);
if (!tailPoint.IsEmpty)
{
drawImage.Draw(new CircleF(tailPoint, 4), new Bgr(Color.Red), 3);
}
if (bodyPoints != null && bodyPoints.Count > 0)
{
Point[] bPoints = bodyPoints.ToArray();
double volume = MathExtension.PolygonArea(bPoints);
Emgu.CV.Structure.Ellipse fittedEllipse = PointCollection.EllipseLeastSquareFitting(bPoints.Select(x => x.ToPointF()).ToArray());
//CvInvoke.Ellipse(drawImage, fittedEllipse.RotatedRect, new MCvScalar(0, 0, 255), 2);
Console.WriteLine("Volume: " + volume + " - " + (fittedEllipse.RotatedRect.Size.Width * fittedEllipse.RotatedRect.Size.Height) + ", Waist Length: " + waistLength);
//Alter this to something better
if (MathExtension.PolygonArea(bPoints) > (rotatedRect.Size.Height * rotatedRect.Size.Width) / 6 || true)
{
//tempImage2.FillConvexPoly(bPoints, new Bgr(Color.White));
tempImage2.DrawPolyline(bPoints, true, new Bgr(Color.White));
PointF centroid = MathExtension.FindCentroid(bPoints);
System.Drawing.Rectangle minRect;
Image <Gray, Byte> temp2 = new Image <Gray, byte>(tempImage2.Width + 2, tempImage2.Height + 2);
CvInvoke.FloodFill(tempImage2, temp2, centroid.ToPoint(), new MCvScalar(255, 255, 255), out minRect, new MCvScalar(5, 5, 5), new MCvScalar(5, 5, 5));
using (Image <Gray, Byte> nonZeroImage = tempImage2.Convert <Gray, Byte>())
{
int[] volume2 = nonZeroImage.CountNonzero();
Console.WriteLine("Volume2: " + volume2[0]);
//int tester = 9;
//using (Image<Gray, Byte> t1 = nonZeroImage.Erode(tester))
//using (Image<Gray, Byte> t2 = t1.Dilate(tester))
//using (Image<Gray, Byte> t3 = t2.Erode(tester))
//using (Image<Gray, Byte> t4 = t3.Dilate(tester))
//using (Image<Gray, Byte> t5 = t4.Erode(tester))
//using (Image<Gray, Byte> t6 = t5.Dilate(tester))
//using (Image<Gray, Byte> t7 = t6.Erode(tester))
//{
// Image6 = ImageService.ToBitmapSource(t7);
//}
}
tempImage2.Draw(new CircleF(centroid, 2), new Bgr(Color.Blue), 2);
double distanceToSpine = double.MaxValue;
PointF p11 = PointF.Empty, p22 = PointF.Empty;
for (int i = 1; i < spineCornerCorrected.Length; i++)
{
PointF point1 = spineCornerCorrected[i - 1];
PointF point2 = spineCornerCorrected[i];
double cDist = MathExtension.MinDistanceFromLineToPoint(point1, point2, centroid);
if (cDist < distanceToSpine)
{
p11 = point1;
p22 = point2;
distanceToSpine = cDist;
}
}
PointSideVector psv = MathExtension.FindSide(p11, p22, centroid);
if (psv == PointSideVector.Below)
{
distanceToSpine *= -1;
}
Console.WriteLine(distanceToSpine + ",");
}
}
for (int i = 1; i < spine.Length; i++)
{
PointF point1 = spine[i - 1];
PointF point2 = spine[i];
point1.X += topCorner.X;
point1.Y += topCorner.Y;
point2.X += topCorner.X;
point2.Y += topCorner.Y;
LineSegment2D line = new LineSegment2D(new Point((int)point1.X, (int)point1.Y), new Point((int)point2.X, (int)point2.Y));
drawImage.Draw(line, new Bgr(Color.Aqua), 2);
tempImage2.Draw(line, new Bgr(Color.Cyan), 2);
}
drawImage.Draw(new CircleF(mouseBlob.Centroid, 2), new Bgr(Color.Blue), 2);
Image3 = ImageService.ToBitmapSource(drawImage);
Image6 = ImageService.ToBitmapSource(tempImage2);
double rotatedRectArea = rotatedRect.Size.Width * rotatedRect.Size.Height;
if (rotatedRectArea < 75000)
{
//Console.WriteLine(rotatedRectArea);
//return;
}
else
{
//Console.WriteLine(rotatedRectArea);
}
double height = rotatedRect.Size.Height;
double width = rotatedRect.Size.Width;
//double angle = rotatedRect.Angle;
bool heightLong = height > width;
double halfLength;
PointF[] vertices = rotatedRect.GetVertices();
if (heightLong)
{
halfLength = height;
}
else
{
halfLength = width;
}
halfLength /= 2;
PointF[] sidePoints1 = new PointF[4], midPoints = new PointF[2];
PointF p1 = vertices[0], p2 = vertices[1], p3 = vertices[2], p4 = vertices[3];
double d1 = p1.DistanceSquared(p2);
double d2 = p2.DistanceSquared(p3);
if (d1 < d2)
{
//p1 and p2, p3 and p4 are side points
sidePoints1[0] = p1;
sidePoints1[1] = p2;
sidePoints1[2] = p4;
sidePoints1[3] = p3;
midPoints[0] = p1.MidPoint(p4);
midPoints[1] = p2.MidPoint(p3);
}
else
{
//p2 and p3, p1 and p4 are side points
sidePoints1[0] = p1;
sidePoints1[1] = p4;
sidePoints1[2] = p2;
sidePoints1[3] = p3;
midPoints[0] = p1.MidPoint(p2);
midPoints[1] = p3.MidPoint(p4);
}
PointF intersection1 = PointF.Empty;
PointF intersection2 = PointF.Empty;
using (Image <Gray, Byte> halfTest1 = origGray.CopyBlank())
using (Image <Gray, Byte> halfTest2 = origGray.CopyBlank())
{
Point[] rect1 = new Point[] { new Point((int)sidePoints1[0].X, (int)sidePoints1[0].Y), new Point((int)midPoints[0].X, (int)midPoints[0].Y), new Point((int)midPoints[1].X, (int)midPoints[1].Y), new Point((int)sidePoints1[1].X, (int)sidePoints1[1].Y) };
Point[] rect2 = new Point[] { new Point((int)sidePoints1[2].X, (int)sidePoints1[2].Y), new Point((int)midPoints[0].X, (int)midPoints[0].Y), new Point((int)midPoints[1].X, (int)midPoints[1].Y), new Point((int)sidePoints1[3].X, (int)sidePoints1[3].Y) };
if (MathExtension.PolygonContainsPoint(rect1, center))
{
//Rect 1 is head, look for line in r2
}
else if (MathExtension.PolygonContainsPoint(rect2, center))
{
//Rect 2 is head, look for line in r1
}
else
{
//Something has gone wrong
}
halfTest1.FillConvexPoly(rect1, new Gray(255));
halfTest2.FillConvexPoly(rect2, new Gray(255));
//Image5 = ImageService.ToBitmapSource(halfTest1);
//Image6 = ImageService.ToBitmapSource(halfTest2);
//binary.Copy(holder1, halfTest1);
//binary.Copy(holder2, halfTest2);
int count1, count2;
//using (Image<Gray, Byte> binaryInverse = subbed.Not())
using (Image <Gray, Byte> holder1 = subbed.Copy(halfTest1))
using (Image <Gray, Byte> holder2 = subbed.Copy(halfTest2))
{
//Image4 = ImageService.ToBitmapSource(subbed);
//Image5 = ImageService.ToBitmapSource(holder1);
//Image6 = ImageService.ToBitmapSource(holder2);
count1 = holder1.CountNonzero()[0];
count2 = holder2.CountNonzero()[0];
}
PointF qr1 = PointF.Empty, qr2 = PointF.Empty, qr3 = PointF.Empty, qr4 = PointF.Empty;
if (count1 > count2)
{
//holder 1 is head, holder 2 is rear
qr1 = sidePoints1[2];
qr2 = sidePoints1[2].MidPoint(midPoints[0]);
qr3 = sidePoints1[3].MidPoint(midPoints[1]);
qr4 = sidePoints1[3];
}
else if (count1 < count2)
{
//holder 2 is head, holder 1 is year
qr1 = sidePoints1[0];
qr2 = sidePoints1[0].MidPoint(midPoints[0]);
qr3 = sidePoints1[1].MidPoint(midPoints[1]);
qr4 = sidePoints1[1];
}
//fat line is qr2, qr3
PointF centerPoint = qr2.MidPoint(qr3);
PointF i1 = qr2;
PointF i2 = qr3;
intersection1 = MathExtension.PolygonLineIntersectionPoint(centerPoint, i1, mouseContour);
intersection2 = MathExtension.PolygonLineIntersectionPoint(centerPoint, i2, mouseContour);
}
double deltaX = halfLength * Math.Cos(rotatedRect.Angle * MathExtension.Deg2Rad);
double deltaY = halfLength * Math.Sin(rotatedRect.Angle * MathExtension.Deg2Rad);
const double scaleFactor = 0.25;
PointF newPoint = new PointF((float)(center.X - (deltaX * scaleFactor)), (float)(center.Y - (deltaY * scaleFactor)));
PointF intersectionPoint1 = PointF.Empty;
PointF intersectionPoint2 = PointF.Empty;
Point[] temp = null;
PointF[] headPoints = RBSKService.RBSKParallel(binary, MouseService.GetStandardMouseRules(), ref temp);
if (headPoints != null)
{
PointF tip = headPoints[2];
//targetPoints = new LineSegment2DF[3];
Point centerInt = new Point((int)newPoint.X, (int)newPoint.Y);
//targetPoints[0] = new LineSegment2DF(centerInt, new PointF(tip.X, tip.Y));
Vector forwardVec = new Vector(tip.X - newPoint.X, tip.Y - newPoint.Y);
Vector rotatedVec = new Vector(-forwardVec.Y, forwardVec.X);
PointF i1 = new PointF((float)(newPoint.X + (rotatedVec.X * 1)), (float)(newPoint.Y + (rotatedVec.Y * 1)));
PointF i2 = new PointF((float)(newPoint.X - (rotatedVec.X * 1)), (float)(newPoint.Y - (rotatedVec.Y * 1)));
//targetPoints[1] = new LineSegment2DF(centerInt, i1);
//targetPoints[2] = new LineSegment2DF(centerInt, i2);
intersectionPoint1 = MathExtension.PolygonLineIntersectionPoint(newPoint, i1, mouseContour);
intersectionPoint2 = MathExtension.PolygonLineIntersectionPoint(newPoint, i2, mouseContour);
}
//displayImage.Draw(mouseBlob.BoundingBox, new Bgr(Color.Red), 2);
displayImage.Draw(new CircleF(mouseBlob.Centroid, 3), new Bgr(Color.Blue), 2);
displayImage.Draw(rotatedRect, new Bgr(Color.Yellow), 3);
//displayImage.Draw(mouseContour, new Bgr(Color.Aqua), 2);
//displayImage.FillConvexPoly(new Point[] { new Point((int)sidePoints1[0].X, (int)sidePoints1[0].Y), new Point((int)midPoints[0].X, (int)midPoints[0].Y), new Point((int)midPoints[1].X, (int)midPoints[1].Y), new Point((int)sidePoints1[1].X, (int)sidePoints1[1].Y) }, new Bgr(Color.Blue));
//if (targetPoints != null)
//{
// displayImage.Draw(targetPoints[0], new Bgr(Color.Green), 2);
// displayImage.Draw(targetPoints[1], new Bgr(Color.Green), 2);
// displayImage.Draw(targetPoints[2], new Bgr(Color.Green), 2);
//}
//if (!intersection1.IsEmpty && !intersection2.IsEmpty)
//{
// LineSegment2DF lineSegment = new LineSegment2DF(intersection1, intersection2);
// displayImage.Draw(lineSegment, new Bgr(Color.MediumPurple), 4);
// //Console.WriteLine(lineSegment.Length);
//}
//displayImage.Draw(new CircleF(newPoint, 4), new Bgr(Color.MediumPurple), 3);
//Console.WriteLine(rotatedRect.Angle);
Image4 = ImageService.ToBitmapSource(displayImage);
}
}
}
///<summary>
/// Constructor that sets current skeleton for the event. </summary>
public SkeletonChangedEventArgs(ISkeleton skeleton)
{
_skeleton = skeleton;
}
public UserSkeletonState()
{
skeleton = new KinectSkeletonWrapper(new Skeleton());
userState = null;
isLeftHandGripped = false;
isRightHandGripped = false;
}
public ISkeleton GetSkeleton(ISkeleton s)
{
return leap.GetSkeleton(s);
}
public static ISkeleton SkeletonSmoothing(ISkeleton s1, ISkeleton s2, int windowSize)
{
var mean = Div(Diff(s1, s2), windowSize);
return(Add(s1, mean));
}
private void FireNewSkeleton(ISkeleton skeleton)
{
NewSkeleton?.Invoke(skeleton);
}
public ISkeleton GetSkeleton(ISkeleton baseSkeleton)
{
return _lastAddedSkeleton;
}
public void SetSkeleton(ISkeleton skeleton)
{
_lastSkeleton = skeleton;
}
private void UpdateSkeleton(KinectV2::Microsoft.Kinect.Body body, ISkeleton newSkeleton)
{
foreach (KeyValuePair<JointType, KinectV2::Microsoft.Kinect.JointType> jointMapping in mapping)
{
var joint = new OrientedJoint
{
JointType = jointMapping.Key,
Point = ToVec3(body.Joints[jointMapping.Value].Position),
Orientation = ToVec4(body.JointOrientations[jointMapping.Value].Orientation),
Valid = body.Joints[jointMapping.Value].TrackingState == KinectV2::Microsoft.Kinect.TrackingState.Tracked
};
newSkeleton.UpdateSkeleton(joint.JointType, joint);
}
}
public void Check(ISkeleton skeleton, ISkeletonReport report)
{
SkeletonCorrectiveAngle correctiveAngle = new SkeletonCorrectiveAngle();
if (Wishness == 0)
{
return;
}
Vector3 firstBone;
Vector3 secondBone;
float currentAngle;
#region defineBonesToUse
//we globally make the vectors point in opposite direction
//ex : when we compare the chest and the vertical vector, the two vectors are pointing up
//ex : when we compare the chest and the head, the head vector points down
//ex : when we talk about chest, the arms and legs point out of the chest
//ex : when we compare lower parts with upper parts : the upper parts point to the chest and the lower parts to wirsts and ankles
//ex : when we compare lower/upper parts : they are pointing out of the chest
//ex : when we talk about legs, the chest point up and, when we talk about arms, it's pointing down
//the sternum define the angle between the shoulders vector and the hips vector -> the trunk torsion
//this is important to correctly compare the angles
switch (Angle)
{
case PlayerJoint.Spine:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter));//chest
secondBone = Vector3.Up;
break;
case PlayerJoint.Sternum:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderRight),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderLeft)); //shoulders (pointing right)
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipRight),
skeleton.GetPositionOf(SkeletonMarkers.HipLeft)); //hips (pointing right)
break;
case PlayerJoint.Head:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter)); //chest
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
skeleton.GetPositionOf(SkeletonMarkers.Head)); //head (pointing down)
break;
case PlayerJoint.ShoulderLeft:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipCenter),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter)); //chest
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowLeft),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderLeft)); //left upper arm
break;
case PlayerJoint.ElbowLeft:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderLeft),
skeleton.GetPositionOf(SkeletonMarkers.ElbowLeft)); //left upper arm
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.WristLeft),
skeleton.GetPositionOf(SkeletonMarkers.ElbowLeft)); //left lower arm
break;
case PlayerJoint.HipLeft:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter)); //chest
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipLeft)); //left upper leg
break;
case PlayerJoint.KneeLeft:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipLeft),
skeleton.GetPositionOf(SkeletonMarkers.KneeLeft)); //left upper leg
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.AnkleLeft),
skeleton.GetPositionOf(SkeletonMarkers.KneeLeft)); //left lower leg
break;
case PlayerJoint.ShoulderRight:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipCenter),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter)); //chest
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowRight),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderRight)); //right upper arm
break;
case PlayerJoint.ShoulderCenter:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowRight),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderRight)); //right upper arm
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ElbowLeft),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderLeft)); //left upper arm
break;
case PlayerJoint.ElbowRight:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderRight),
skeleton.GetPositionOf(SkeletonMarkers.ElbowRight)); //right upper arm
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.WristRight),
skeleton.GetPositionOf(SkeletonMarkers.ElbowRight)); //right lower arm
break;
case PlayerJoint.HipRight:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter),
skeleton.GetPositionOf(SkeletonMarkers.HipCenter)); //chest
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeRight),
skeleton.GetPositionOf(SkeletonMarkers.HipRight)); //right upper leg
break;
case PlayerJoint.HipCenter:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipLeft)); //left upper leg
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.KneeRight),
skeleton.GetPositionOf(SkeletonMarkers.HipRight)); //right upper leg
break;
case PlayerJoint.KneeRight:
firstBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipRight),
skeleton.GetPositionOf(SkeletonMarkers.KneeRight)); //left upper leg
secondBone = Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.AnkleRight),
skeleton.GetPositionOf(SkeletonMarkers.KneeRight)); //left lower leg
break;
default:
firstBone = Vector3.Zero;
secondBone = Vector3.Zero;
break;
}
#endregion
#region check the angle
currentAngle = MathHelper.ToDegrees(GetAngleFromVectors(firstBone, secondBone));
if (Wishness > 0 && Math.Abs(currentAngle - WishedAngle) > Threshold)
{ //we want to reach this position and we currently don't -> wrong
correctiveAngle = new SkeletonCorrectiveAngle(Angle, currentAngle, WishedAngle, Threshold,
Wishness, restrictionPlane, true);
}
else if (Wishness < 0 && Math.Abs(currentAngle - WishedAngle) < Threshold)
{ //we don't want to reach this position but we currently do -> wrong
correctiveAngle = new SkeletonCorrectiveAngle(Angle, currentAngle, WishedAngle, Threshold,
Wishness, restrictionPlane, true);
}
#endregion
#region check if in the right plane
//the idea is, now, to check if the bones used are, more or less, in the choosen plane
//we will mesure the angle between the normal to the plane and the normal to the two vectors which forms the angle
//this angle will give us an information about the respect of this restriction plane
//create the frontal plane
Plane plane = new Plane(skeleton.GetPositionOf(SkeletonMarkers.HipRight),
skeleton.GetPositionOf(SkeletonMarkers.HipLeft),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter));
//define the plane to use
switch (restrictionPlane)
{
case physioPlanes.NONE:
return;
case physioPlanes.FRONTAL:
break;
case physioPlanes.SAGITTAL:
//that's the plane defined by the normal to the frontal plane and the trunk vector
//so we take the cross of these two vectors -> that define the normal to the plane
//the distance from the origine along these normal doesn't matter for us
plane = new Plane(Vector3.Cross(plane.Normal,
Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipCenter),
skeleton.GetPositionOf(SkeletonMarkers.ShoulderCenter))),
0);
break;
case physioPlanes.TRANSVERSE:
//the same principle is in application here
plane = new Plane(Vector3.Cross(plane.Normal,
Vector3.Subtract(skeleton.GetPositionOf(SkeletonMarkers.HipLeft),
skeleton.GetPositionOf(SkeletonMarkers.HipRight))),
0);
break;
default:
break;
}
//basically, we are checking the angle with any plane parallel to the restriction plane
float angleWithPlane = MathHelper.ToDegrees(GetAngleFromVectors(plane.Normal, Vector3.Cross(firstBone, secondBone)));
float thresholdPlane = 15;
if ((Math.Abs(angleWithPlane) > thresholdPlane) && (Math.Abs(angleWithPlane - 180) > thresholdPlane))
{ //we don't respect the restriction plane
correctiveAngle = new SkeletonCorrectiveAngle(Angle, currentAngle, WishedAngle, Threshold,
Wishness, restrictionPlane, false);
}
#endregion
if (correctiveAngle.Severity != 0) //so the correctiveAngle has been assigned
{
if (report.GeneralSeverity.CompareTo(Math.Abs(Wishness)) < 0)
{
report.GeneralSeverity = Math.Abs(Wishness);
}
report.CorrectiveCollection.CorrectiveItems.Add(correctiveAngle);
}
}
/// <summary>
/// Determines whether the specified <see cref="ISkeleton"/> is equal to the current <see cref="Trame.Implementation.Skeleton.Skeleton"/>.
/// </summary>
/// <param name="other">The <see cref="ISkeleton"/> to compare with the current <see cref="Trame.Implementation.Skeleton.Skeleton"/>.</param>
/// <returns><c>true</c> if the specified <see cref="ISkeleton"/> is equal to the current
/// <see cref="Trame.Implementation.Skeleton.Skeleton"/>; otherwise, <c>false</c>.</returns>
public bool Equals(ISkeleton other)
{
return(valid == other.Valid && _joints.Values.Equals(other.Joints));
}
/// <summary>
/// Fires the new skeleton.
/// </summary>
/// <param name="s">S.</param>
private void FireNewSkeleton(ISkeleton s)
{
if (NewSkeleton != null)
{
NewSkeleton(s);
}
}
//here we go, thats the method called by vvvv each frame
//all data handling should be in here
public void Evaluate(int SpreadMax)
{
//if any of the inputs has changed
//recompute the outputs
if (FJointNameInput.PinIsChanged)
{
string name;
FJointNameInput.GetString(0, out name);
if (!string.IsNullOrEmpty(name))
{
FRootJoint.Name = name;
FSkeleton.BuildJointTable();
FSkeletonOutput.MarkPinAsChanged();
}
}
if (FChildPins.Any(c => c.PinIsChanged))
{
FSkeleton.ClearAll();
FSkeleton.Root = FRootJoint;
FSkeleton.BuildJointTable();
for (int i = 0; i < FChildPins.Count; i++)
{
if (true) //childPinsList[i].PinIsChanged)
{
FSkeletonOutput.MarkPinAsChanged();
if (FChildPins[i].IsConnected)
{
object currInterface;
FChildPins[i].GetUpstreamInterface(out currInterface);
ISkeleton subSkeleton = (ISkeleton)currInterface;
IJoint child = subSkeleton.Root.DeepCopy();
FSkeleton.InsertJoint(FSkeleton.Root.Name, child);
FSkeleton.BuildJointTable();
}
}
}
// re-calculate the IDs ...
int currId = 0;
foreach (KeyValuePair <string, IJoint> pair in FSkeleton.JointTable)
{
pair.Value.Id = currId;
currId++;
}
}
if (FBaseTransformInput.PinIsChanged)
{
Matrix4x4 baseTransform;
FBaseTransformInput.GetMatrix(0, out baseTransform);
FRootJoint.BaseTransform = baseTransform;
FSkeletonOutput.MarkPinAsChanged();
}
if (FRotationConstraintsInput.PinIsChanged)
{
FRootJoint.Constraints.Clear();
for (int i = 0; i < 3; i++)
{
double from, to;
FRotationConstraintsInput.GetValue2D(i, out from, out to);
FRootJoint.Constraints.Add(new Vector2D(from, to));
}
FSkeletonOutput.MarkPinAsChanged();
}
FSkeletonOutput.SetInterface(FSkeleton);
}
/// <summary>
/// Initializes a new instanKinectDevice="Device.KinectDevice"/> class.
/// </summary>
public KinectDevice()
{
_lastSkeleton = Creator.GetNewDefaultSkeleton<InMapSkeleton>();
}
private void TrameOnNewSkeleton(ISkeleton skeleton)
{
Console.WriteLine("Found new skeleton with id {0}.", skeleton.ID);
_skeletons.Add(skeleton);
}
/// <summary>
/// Raises the frame arrived event.
/// </summary>
/// <param name="sender">Sender.</param>
/// <param name="e">E.</param>
private void OnFrameArrived(object sender, SkeletonFrameReadyEventArgs e)
{
using (var frame = e.OpenSkeletonFrame())
{
if (frame == null)
{
return;
}
var foundedSkeletons = new KinectV1::Microsoft.Kinect.Skeleton[frame.SkeletonArrayLength];
frame.CopySkeletonDataTo(foundedSkeletons);
var skeletons = foundedSkeletons.Where(s => s.TrackingState == SkeletonTrackingState.Tracked);
foreach (var skeleton in skeletons)
{
var trameSkeleton = CreateSkeleton(skeleton);
_lastSkeleton = trameSkeleton;
FireNewSkeleton(trameSkeleton);
}
}
}
public void SetMetricsAndCreateChildrenFromXml(ISkeleton skeleton, XElement boneData)
{
_relativePosition = new Vector2((float)boneData.Attribute("offset-x"), (float)boneData.Attribute("offset-y"));
_relativeRotation = boneData.Attribute("rotation") == null ? 0.0f : MathHelper.ToRadians((float)boneData.Attribute("rotation"));
_relativeScale = boneData.Attribute("scale") == null ? 1.0f : (float)boneData.Attribute("scale");
_relativeDepth = (float)boneData.Attribute("depth");
_relativeTint = Color.White;
if (boneData.Elements("bone") != null)
{
foreach (XElement bone in boneData.Elements("bone"))
{
StorableBone newBone = new StorableBone();
newBone.ID = bone.Attribute("id").Value;
skeleton.AddBone(newBone, boneData.Attribute("id").Value);
newBone.SetMetricsAndCreateChildrenFromXml(skeleton, bone);
}
}
}
public SkeletonExtension(ISkeleton extendingFrame, ISkeleton baseFrame)
{
this.extendingFrame = extendingFrame;
this.baseFrame = baseFrame;
if (this.baseFrame == null) { this.baseFrame = this; }
}