public TrackingStateGait getJointPositionFromGround(JointTypeGait joint, bool checkTracked, out float X, out float Y, out float Z)
{
var t = getJointPosition(joint, checkTracked, out X, out Y, out Z);
Y = dot(X, Y, Z, GroundX, GroundY, GroundZ) + GroundW;
return(t);
}
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);
}
public double getJointPositionProjection(JointTypeGait joint, float w, float x, float y, float z, bool checkTracked)
{
float jointX, jointY, jointZ;
getJointPosition(joint, checkTracked, out jointX, out jointY, out jointZ);
if (jointX == missingX && jointY == missingY && jointZ == missingZ)
{
return(-100);
}
return(dot(jointX, jointY, jointZ, x, y, z) + w);
}
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
}
public void getJointPositionInBodyCoordinates(JointTypeGait joint, bool checkTracked, out float xRes, out float yRes, out float zRes)
{
getJointPositionInBodyCoordinates(joint, GroundW, GroundX, GroundY, GroundZ, checkTracked, out xRes, out yRes, out zRes);
}
