public static float getActiveAngle(IKSolverServo.JointEntity jointEntity)
{
Vector3 euler = jointEntity.Joint.localEulerAngles;
JointActiveAngle jointActiveAngle = jointEntity.activeJointAngle;
if (jointActiveAngle == JointActiveAngle.X)
{
return(euler.x);
}
else if (jointActiveAngle == JointActiveAngle.Y)
{
return(euler.y);
}
return(euler.z);
}
// Convert the current active angle from a joint to a servo angle
private static int AngleJoint2Servo(IKSolverServo.JointEntity joint)
{
float jointAngle = WrapAngle(getActiveAngle(joint));
float in_min = joint.angleMin;
float in_max = joint.angleMax;
float out_min = joint.servoMin;
float out_max = joint.servoMax;
// Account for inverted axis
if (joint.invertedAngles)
{
out_min *= -1;
out_max *= -1;
}
return(Mathf.RoundToInt(map(jointAngle, in_min, in_max, out_min, out_max)));
}
