public TrackingStateGait getJointPosition(JointTypeGait joint, bool checkTracked, out float X, out float Y, out float Z)
{
if (Joints == null)
{
X = missingX;
Y = missingY;
Z = missingZ;
return(TrackingStateGait.NotTracked);
}
if (checkTracked)
{
int s = Joints.Select(x => (x.TrackingState == TrackingStateGait.Tracked) ? 0 : 1).Sum();
if (s > 5)
{
X = missingX;
Y = missingY;
Z = missingZ;
return(TrackingStateGait.NotTracked);
}
}
var jointPos = Joints[(int)joint];
X = jointPos.X;
Y = jointPos.Y;
Z = jointPos.Z;
return(jointPos.TrackingState);
}
private void InitLazyProps()
{
this._ModelData = new Lazy <ModelBase>(() => _ModelDataInitializer());
this._Vertices = new Lazy <Vertex[]>(() => Triangles.SelectMany(t => t).Union(new Vertex[0]).ToArray());
this._Textures = new Lazy <Texture[]>(() => Triangles.Select(t => t.Texture).Union(new Texture[0]).ToArray());
this._Joints = new Lazy <Joint[]>(() => RootJoint == null ? new Joint[0] : RootJoint.GetSelfAndDescendents().ToArray());
this._Animations = new Lazy <AnimationSequence[]>(() => Joints.Select(j => j.Animation).ToArray());
this._IsVisible = new Lazy <bool>(() => Textures.Where(t => ExcludeTexturePrefixes.Count(v => t.Name.StartsWith(v)) == 0).Count() != 0);
this._IsAnimated = new Lazy <bool>(() => Animations.Count() != 0 && NumAnimationKeyFrames != 0);
this._HasMultipleTextures = new Lazy <bool>(() => Textures.Count() > 1);
this._HasGeometry = new Lazy <bool>(() => Triangles.Count() != 0);
this._HasNormals = new Lazy <bool>(() => HasGeometry && Vertices.First().Normal != Vector4.Zero);
this._HasTangents = new Lazy <bool>(() => HasGeometry && Vertices.First().Tangent != Vector4.Zero);
this._HasBinormals = new Lazy <bool>(() => HasGeometry && Vertices.First().Binormal != Vector4.Zero);
this._HasSkeleton = new Lazy <bool>(() => RootJoint != null);
this._NumJoints = new Lazy <int>(() => Joints.Count());
this._NumAnimationFrames = new Lazy <int>(() => Animations.Select(a => a.NumFrames).Union(new[] { 0 }).Max());
this._NumAnimationKeyFrames = new Lazy <int>(() => Animations.Select(anim => anim.Frames.Select(f => f.FrameNum)).Aggregate((a, v) => a.Union(v)).Count());
}
public void getJointPositionInBodyCoordinates(JointTypeGait joint, float w, float x, float y, float z, bool checkTracked, out float xRes, out float yRes, out float zRes)
{
if (Joints == null)
{
xRes = -100; yRes = -100; zRes = -100;
return;
}
if (checkTracked)
{
int s = Joints.Select(f => (f.TrackingState == TrackingStateGait.Tracked) ? 0 : 1).Sum();
if (s > 5)
{
xRes = -100; yRes = -100; zRes = -100;
return;
}
}
Vector ground = new Vector(w, x, y, z);
var jointPos = Joints[(int)joint];
var hipLeft = Joints[(int)JointTypeGait.HipLeft];
var hipRight = Joints[(int)JointTypeGait.HipRight];
var hipCenter = (Sensor == SensorType.One) ? Joints[(int)JointTypeGait.SpineBase] : Joints[(int)JointTypeGait.HipCenter];
Vector vw = diff(jointPos, hipCenter);
Vector hipDirection = diff(hipLeft, hipRight);
yRes = dot(vw, ground);
vw = orthogonal(vw, ground);
hipDirection = orthogonal(hipDirection, ground);
float norm = (float)Math.Sqrt(dot(hipDirection, hipDirection));
xRes = dot(vw, hipDirection) / norm;
vw = orthogonal(vw, hipDirection); //Left with Z direction
zRes = (float)Math.Sqrt(dot(vw, vw)) * ((vw.Z > 0) ? 1 : -1); //Get norm and maintain sign
}