/// <summary>
/// Handles the body frame data arriving from the sensor
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
//private
point3DDouble getVector(point3DDouble a, point3DDouble b)
{
point3DDouble returnValue = new point3DDouble();
returnValue.X = b.X - a.X;
returnValue.Y = b.Y - a.Y;
returnValue.Z = b.Z - a.Z;
return(returnValue);
}
double returnAngleBetweenJoints(CameraSpacePoint a, CameraSpacePoint b, CameraSpacePoint c)
{
point3DDouble vector1 = new point3DDouble();
point3DDouble vector2 = new point3DDouble();
vector1 = getVector(new point3DDouble(a.X, a.Y, a.Z), new point3DDouble(b.X, b.Y, b.Z));
vector2 = getVector(new point3DDouble(a.X, a.Y, a.Z), new point3DDouble(c.X, c.Y, c.Z));
double returnValue = getAngle(vector1, vector2);
return(returnValue);
}
double getAngle(point3DDouble v1, point3DDouble v2)
{
double dotProduct = (v1.X * v2.X) + (v1.Y * v2.Y) + (v1.Z * v2.Z);
double mag1 = (v1.X * v1.X) + (v1.Y * v1.Y) + (v1.Z * v1.Z);
double mag2 = (v2.X * v2.X) + (v2.Y * v2.Y) + (v2.Z * v2.Z);
mag1 = Math.Sqrt(mag1);
mag2 = Math.Sqrt(mag2);
double angle = Math.Acos(dotProduct / (mag1 * mag2));
angle = angle * 180 / pi;
return(angle);
}
