private void OnLowerBodyAnglesChanged(HumanoidAngles <Vector3> absoluteAngles)
{
var headsetRotation = Quaternion.identity;
var initialWaistYaw = WaistBaseOrientation.z;
var initialWaistAngles = Quaternion.AngleAxis(initialWaistYaw, Vector3.up);
_yawAdjustedWaistAngles[0] = absoluteAngles.Waist.x;
_yawAdjustedWaistAngles[1] = absoluteAngles.Waist.y;
_yawAdjustedWaistAngles[2] = absoluteAngles.Waist.z;
_yawAdjustedLeftUpperLegAngles[0] = absoluteAngles.LeftUpperLeg.x;
_yawAdjustedLeftUpperLegAngles[1] = absoluteAngles.LeftUpperLeg.y;
_yawAdjustedLeftUpperLegAngles[2] = absoluteAngles.LeftUpperLeg.z;
_yawAdjustedLeftLowerLegAngles[0] = absoluteAngles.LeftLowerLeg.x;
_yawAdjustedLeftLowerLegAngles[1] = absoluteAngles.LeftLowerLeg.y;
_yawAdjustedLeftLowerLegAngles[2] = absoluteAngles.LeftLowerLeg.z;
_yawAdjustedRightUpperLegAngles[0] = absoluteAngles.RightUpperLeg.x;
_yawAdjustedRightUpperLegAngles[1] = absoluteAngles.RightUpperLeg.y;
_yawAdjustedRightUpperLegAngles[2] = absoluteAngles.RightUpperLeg.z;
_yawAdjustedRightLowerLegAngles[0] = absoluteAngles.RightLowerLeg.x;
_yawAdjustedRightLowerLegAngles[1] = absoluteAngles.RightLowerLeg.y;
_yawAdjustedRightLowerLegAngles[2] = absoluteAngles.RightLowerLeg.z;
// Transform absolute lower body angles into relative ones
var localAngleWaist = _jointRotations.rotateWaist(
_yawAdjustedWaistAngles,
initialWaistAngles,
headsetRotation
);
var localAngleLeftUpperLeg = _jointRotations.rotateLeftLeg(
_yawAdjustedLeftUpperLegAngles,
localAngleWaist,
initialWaistAngles
);
var localAngleLeftLowerLeg = _jointRotations.rotateLeftShin(
_yawAdjustedLeftLowerLegAngles,
localAngleWaist,
localAngleLeftUpperLeg,
initialWaistAngles
);
var localAngleRightUpperLeg = _jointRotations.rotateRightLeg(
_yawAdjustedRightUpperLegAngles,
localAngleWaist,
initialWaistAngles
);
var localAngleRightLowerLeg = _jointRotations.rotateRightShin(
_yawAdjustedRightLowerLegAngles,
localAngleWaist,
localAngleRightUpperLeg,
initialWaistAngles
);
LocalAngles.SetLowerBodyAngles(localAngleWaist, localAngleLeftUpperLeg, localAngleLeftLowerLeg,
localAngleRightUpperLeg, localAngleRightLowerLeg);
}
/// <summary>
/// Applies angles to the lower body all at once.
/// </summary>
/// <param name="lowerBodyAngles"></param>
public void SetLowerBodyAngles(HumanoidAngles <T> lowerBodyAngles)
{
_waist = lowerBodyAngles._waist;
_leftUpperLeg = lowerBodyAngles._leftUpperLeg;
_leftLowerLeg = lowerBodyAngles._leftLowerLeg;
_rightUpperLeg = lowerBodyAngles._rightUpperLeg;
_rightLowerLeg = lowerBodyAngles._rightLowerLeg;
RaiseLowerBodyAnglesChangedEvent();
}
/// <summary>
/// Applies angles to the upper body all at once.
/// </summary>
/// <param name="upperBodyAngles"></param>
public void SetUpperBodyAngles(HumanoidAngles <T> upperBodyAngles)
{
_chest = upperBodyAngles._chest;
_leftUpperArm = upperBodyAngles._leftUpperArm;
_leftLowerArm = upperBodyAngles._leftLowerArm;
_rightUpperArm = upperBodyAngles._rightUpperArm;
_rightLowerArm = upperBodyAngles._rightLowerArm;
RaiseUpperBodyAnglesChangedEvent();
}
private void OnUpperBodyAnglesChanged(HumanoidAngles <Vector3> absoluteAngles)
{
var headsetRotation = Quaternion.identity;
// Pack absolute angles into arrays for calculations
var baseChestYaw = ChestBaseOrientation.z;
_baseChestAngles[0] = ChestBaseOrientation.x;
_baseChestAngles[1] = ChestBaseOrientation.y;
_baseChestAngles[2] = ChestBaseOrientation.z;
_yawAdjustedChestAngles[0] = absoluteAngles.Chest.x;
_yawAdjustedChestAngles[1] = absoluteAngles.Chest.y;
_yawAdjustedChestAngles[2] = absoluteAngles.Chest.z - baseChestYaw;
_yawAdjustedLeftUpperArmAngles[0] = absoluteAngles.LeftUpperArm.x;
_yawAdjustedLeftUpperArmAngles[1] = absoluteAngles.LeftUpperArm.y;
_yawAdjustedLeftUpperArmAngles[2] = absoluteAngles.LeftUpperArm.z - baseChestYaw;
_yawAdjustedLeftLowerArmAngles[0] = absoluteAngles.LeftLowerArm.x;
_yawAdjustedLeftLowerArmAngles[1] = absoluteAngles.LeftLowerArm.y;
_yawAdjustedLeftLowerArmAngles[2] = absoluteAngles.LeftLowerArm.z - baseChestYaw;
_yawAdjustedRightUpperArmAngles[0] = absoluteAngles.RightUpperArm.x;
_yawAdjustedRightUpperArmAngles[1] = absoluteAngles.RightUpperArm.y;
_yawAdjustedRightUpperArmAngles[2] = absoluteAngles.RightUpperArm.z - baseChestYaw;
_yawAdjustedRightLowerArmAngles[0] = absoluteAngles.RightLowerArm.x;
_yawAdjustedRightLowerArmAngles[1] = absoluteAngles.RightLowerArm.y;
_yawAdjustedRightLowerArmAngles[2] = absoluteAngles.RightLowerArm.z - baseChestYaw;
// Transform absolute upper body angles into local ones
var localAngleChest = _jointRotations.rotateCore(
_yawAdjustedChestAngles,
_baseChestAngles,
headsetRotation
);
var localAngleLeftUpperArm = _jointRotations.rotateLeftArm(
_yawAdjustedLeftUpperArmAngles,
localAngleChest,
headsetRotation
);
var localAngleLeftLowerArm = _jointRotations.rotateLeftForearm(
_yawAdjustedLeftLowerArmAngles,
localAngleChest,
localAngleLeftUpperArm,
headsetRotation
);
var localAngleRightUpperArm = _jointRotations.rotateRightArm(
_yawAdjustedRightUpperArmAngles,
localAngleChest,
headsetRotation
);
var localAngleRightLowerArm = _jointRotations.rotateRightForearm(
_yawAdjustedRightLowerArmAngles,
localAngleChest,
localAngleRightUpperArm,
headsetRotation
);
LocalAngles.SetUpperBodyAngles(localAngleChest, localAngleLeftUpperArm, localAngleLeftLowerArm,
localAngleRightUpperArm, localAngleRightLowerArm);
}
/// <summary>
/// Applies angles to the entire body all at once.
/// </summary>
/// <param name="fullBodyAngles"></param>
public void SetFullBodyAngles(HumanoidAngles <T> fullBodyAngles)
{
SetUpperBodyAngles(fullBodyAngles);
SetLowerBodyAngles(fullBodyAngles);
}
