/// <summary>
/// Computes the angle of your shoulder to enpoint flattened onto the unit-vector axis
/// </summary>
/// <param name="endpoint"></param>
/// <param name="unitVector"></param>
/// <returns></returns>
protected double computeArmAngle(Point3D endpoint, Point3D unitVector)
{
Point3D shoulderLocation = this.getShoulderLocation();
Point3D shoulderToEndpoint = shoulderLocation.subtract(endpoint);
double angle = unitVector.calculateAngle(shoulderToEndpoint);
return angle;
}
protected double getArmAngle(Point3D endpoint)
{
Point3D xUnitVector = new Point3D(1, 0, 0);
Point3D shoulderLocation = this.getShoulderLocation();
Point3D shoulderToEndpoint = shoulderLocation.subtract(endpoint);
shoulderToEndpoint.Y = 0;
double angle = xUnitVector.calculateAngle(shoulderToEndpoint);
return angle;
}
