public void OnNewBodyFrame <S, T>(S bodyStream, T bodyFrame)
{
var frame = bodyFrame as Astra.BodyFrame;
var stream = bodyStream as Astra.BodyStream;
if (frame != null && stream != null)
{
if (frame.Width == 0 ||
frame.Height == 0)
{
return;
}
if (bodyFrameIndex == frame.FrameIndex)
{
return;
}
bodyFrameIndex = frame.FrameIndex;
frame.CopyBodyData(ref astraBodies);
var bodies = new BodyJoints[numberOfBodies];
for (int i = 0; i < this.numberOfBodies; i++)
{
bodies[i] = GetBody(astraBodies[i]);
}
foreach (var receiver in BodyReceivers)
{
receiver.ReceiveData(bodies);
}
}
}
public BodyJoints GetBody <T>(T source)
{
var astra = source as Astra.Body;
var id = astra.Id;
Status status = Status.NotTracking;
Dictionary <JointType, Joint> joints = new Dictionary <JointType, Joint>();
switch (astra.Status)
{
case Astra.BodyStatus.Tracking:
status = Status.Tracking;
break;
default:
status = Status.NotTracking;
break;
}
if (status == Status.Tracking)
{
for (int i = 0; i < astra.Joints.Length; i++)
{
var joint = GetJoint(astra.Joints[i]);
joints.Add(joint.type, joint);
}
}
var body = new BodyJoints(id, status, joints);
return(body);
}
public void Assign(BodyJoints destJoint)
{
selectedJoint = destJoint;
if (Assignments.ContainsKey(destJoint))
{
var dja = Assignments[destJoint];
CurrentSelectedJointPlugin = dja.PluginName;
CurrentSelectedJointSensor = dja.SensorName;
DispatcherTimer temp = new DispatcherTimer
{
IsEnabled = true,
Interval = TimeSpan.FromMilliseconds(100),
};
temp.Tick += (sender, args) =>
{
CurrentSelectedJointPlugin = dja.PluginName;
CurrentSelectedJointSensor = dja.SensorName;
temp.Stop();
};
temp.Start();
}
}
float CenterOfGravity(BodyJoints body, float myMass)
{
Vector3 v = new Vector3(body.joints[JointType.Head].worldPosition.x,
body.joints[JointType.Head].worldPosition.y, body.joints[JointType.Head].worldPosition.z);
float mass = myMass; //kg
float leftM, rightM;
var vectors = MovementAnalyzer.GenerateLowerBodyPlanes(body);
Vector3 sFrame = vectors[BasePlanes.Frontal];
float A = sFrame.x,
B = sFrame.y,
C = sFrame.z;
float angle = (float)Math.Asin(Math.Abs(A * v.x + B * v.y + C * v.z));
if (angle == 90.0f)
{
leftM = mass / 2.0f;
rightM = mass / 2.0f;
}
float distanceFrom90 = 90.0f - angle;
if (distanceFrom90 > 0)
{
float leftMultiple = distanceFrom90 / 90.0f;
leftM = mass * leftMultiple;
rightM = mass - leftM;
}
else
{
float rightMultiple = distanceFrom90 / 90.0f;
rightM = rightMultiple * mass;
leftM = mass - rightMultiple;
}
// Calculado em relação a cabeça!
var footLeft = body.joints[JointType.LeftFoot].worldPosition;
var footRight = body.joints[JointType.RightFoot].worldPosition;
float distanceFromDataumLeft = (float)Math.Sqrt(
Math.Pow(v.x - footLeft.x, 2.0)
+ Math.Pow(v.y - footLeft.y, 2.0)
+ Math.Pow(v.z - footLeft.z, 2.0));
float distanceFromDataumRight = (float)Math.Sqrt(
Math.Pow(v.x - footRight.x, 2.0)
+ Math.Pow(v.y - footRight.y, 2.0)
+ Math.Pow(v.z - footRight.z, 2.0));
float momentLeft = distanceFromDataumLeft * leftM;
float momentRight = distanceFromDataumRight * rightM;
float momentSum = momentLeft + momentRight;
return(momentSum / mass);
}
private void RadioButton_Checked(object sender, RoutedEventArgs e)
{
string selectedDestJoin = ((RadioButton)sender).Content.ToString();
Debug.WriteLine(selectedDestJoin);
string t = selectedDestJoin.Replace("(", "_").Replace(")", "");
BodyJoints petType = (BodyJoints)Enum.Parse(typeof(BodyJoints), t);
((MainWindowViewModel)this.DataContext).Assign(petType);
}
public BodyAngles(BodyJoints body)
{
FrontalAngles = new PlaneBasedBodyAngles(body, BasePlanes.Frontal);
SagittalAngles = new PlaneBasedBodyAngles(body, BasePlanes.Sagittal);
HorizontalAngles = new PlaneBasedBodyAngles(body, BasePlanes.Horizontal);
}
// Start is called before the first frame update
void Start()
{
bj = manager.GetComponent <BodyJoints>();
}
