/// <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); }