public void CalibrateFullBodyTrackingAuto()
{
_logger.Info("Calibrating full body tracking");
CalibrationData.FullBodyCalibration fullBodyCalibration = _calibrationData.automaticCalibration;
Vector3 floorNormal = Vector3.up;
float floorPosition = _settings.moveFloorWithRoomAdjust ? _roomCenter.y : 0;
if (_trackedDeviceManager.leftFoot.tracked)
{
TrackedDeviceState leftFoot = _trackedDeviceManager.leftFoot;
Vector3 leftFootForward = leftFoot.rotation * Vector3.up; // forward on feet trackers is y (up)
Vector3 leftFootStraightForward = Vector3.ProjectOnPlane(leftFootForward, floorNormal); // get projection of forward vector on xz plane (floor)
Quaternion leftRotationCorrection = Quaternion.Inverse(leftFoot.rotation) * Quaternion.LookRotation(Vector3.up, leftFootStraightForward); // get difference between world rotation and flat forward rotation
fullBodyCalibration.leftFoot = new Pose((leftFoot.position.y - floorPosition) * Vector3.back, leftRotationCorrection);
_logger.Info("Set left foot pose correction " + fullBodyCalibration.leftFoot);
}
if (_trackedDeviceManager.rightFoot.tracked)
{
TrackedDeviceState rightFoot = _trackedDeviceManager.rightFoot;
Vector3 rightFootForward = rightFoot.rotation * Vector3.up;
Vector3 rightFootStraightForward = Vector3.ProjectOnPlane(rightFootForward, floorNormal);
Quaternion rightRotationCorrection = Quaternion.Inverse(rightFoot.rotation) * Quaternion.LookRotation(Vector3.up, rightFootStraightForward);
fullBodyCalibration.rightFoot = new Pose((rightFoot.position.y - floorPosition) * Vector3.back, rightRotationCorrection);
_logger.Info("Set right foot pose correction " + fullBodyCalibration.rightFoot);
}
if (_trackedDeviceManager.head.tracked && _trackedDeviceManager.waist.tracked)
{
TrackedDeviceState head = _trackedDeviceManager.head;
TrackedDeviceState pelvis = _trackedDeviceManager.waist;
// using "standard" 8 head high body proportions w/ eyes at 1/2 head height
// reference: https://miro.medium.com/max/3200/1*cqTRyEGl26l4CImEmWz68Q.jpeg
float eyeHeight = head.position.y - floorPosition;
Vector3 wantedPelvisPosition = new Vector3(0, eyeHeight / 22.5f * 14f, 0);
Vector3 pelvisPositionCorrection = wantedPelvisPosition - Vector3.up * (pelvis.position.y - floorPosition);
Vector3 pelvisForward = pelvis.rotation * Vector3.forward;
Vector3 pelvisStraightForward = Vector3.ProjectOnPlane(pelvisForward, floorNormal);
Quaternion pelvisRotationCorrection = Quaternion.Inverse(pelvis.rotation) * Quaternion.LookRotation(pelvisStraightForward, Vector3.up);
fullBodyCalibration.waist = new Pose(pelvisPositionCorrection, pelvisRotationCorrection);
_logger.Info("Set waist pose correction " + fullBodyCalibration.waist);
}
}
public void CalibrateFullBodyTracking()
{
Plugin.logger.Info("Calibrating full body tracking");
TrackedDeviceManager input = PersistentSingleton <TrackedDeviceManager> .instance;
TrackedDeviceState head = input.head;
TrackedDeviceState leftFoot = input.leftFoot;
TrackedDeviceState rightFoot = input.rightFoot;
TrackedDeviceState pelvis = input.waist;
Vector3 floorNormal = Vector3.up;
float floorPosition = SettingsManager.settings.moveFloorWithRoomAdjust ? BeatSaberUtil.GetRoomCenter().y : 0;
if (leftFoot.found)
{
Vector3 leftFootForward = leftFoot.rotation * Vector3.up; // forward on feet trackers is y (up)
Vector3 leftFootStraightForward = Vector3.ProjectOnPlane(leftFootForward, floorNormal); // get projection of forward vector on xz plane (floor)
Quaternion leftRotationCorrection = Quaternion.Inverse(leftFoot.rotation) * Quaternion.LookRotation(Vector3.up, leftFootStraightForward); // get difference between world rotation and flat forward rotation
SettingsManager.settings.fullBodyCalibration.leftLeg = new Pose((leftFoot.position.y - floorPosition) * Vector3.down, leftRotationCorrection);
Plugin.logger.Info("Saved left foot pose correction " + SettingsManager.settings.fullBodyCalibration.leftLeg);
}
if (rightFoot.found)
{
Vector3 rightFootForward = rightFoot.rotation * Vector3.up;
Vector3 rightFootStraightForward = Vector3.ProjectOnPlane(rightFootForward, floorNormal);
Quaternion rightRotationCorrection = Quaternion.Inverse(rightFoot.rotation) * Quaternion.LookRotation(Vector3.up, rightFootStraightForward);
SettingsManager.settings.fullBodyCalibration.rightLeg = new Pose((rightFoot.position.y - floorPosition) * Vector3.down, rightRotationCorrection);
Plugin.logger.Info("Saved right foot pose correction " + SettingsManager.settings.fullBodyCalibration.rightLeg);
}
if (head.found && pelvis.found)
{
// using "standard" 8 head high body proportions w/ eyes at 1/2 head height
// reference: https://miro.medium.com/max/3200/1*cqTRyEGl26l4CImEmWz68Q.jpeg
float eyeHeight = head.position.y - floorPosition;
Vector3 wantedPelvisPosition = new Vector3(0, eyeHeight / 15f * 10f, 0);
Vector3 pelvisPositionCorrection = wantedPelvisPosition - Vector3.up * (pelvis.position.y - floorPosition);
Vector3 pelvisForward = pelvis.rotation * Vector3.forward;
Vector3 pelvisStraightForward = Vector3.ProjectOnPlane(pelvisForward, floorNormal);
Quaternion pelvisRotationCorrection = Quaternion.Inverse(pelvis.rotation) * Quaternion.LookRotation(pelvisStraightForward, Vector3.up);
SettingsManager.settings.fullBodyCalibration.pelvis = new Pose(pelvisPositionCorrection, pelvisRotationCorrection);
Plugin.logger.Info("Saved pelvis pose correction " + SettingsManager.settings.fullBodyCalibration.pelvis);
}
}
public void CalibrateFullBodyTrackingManual(SpawnedAvatar spawnedAvatar)
{
CalibrationData.FullBodyCalibration fullBodyCalibration = _calibrationData.GetAvatarManualCalibration(spawnedAvatar.avatar.fileName);
if (_trackedDeviceManager.waist.tracked)
{
TrackedDeviceState pelvis = _trackedDeviceManager.waist;
Vector3 positionOffset = Quaternion.Inverse(spawnedAvatar.pelvis.rotation) * (spawnedAvatar.pelvis.position - ApplyTrackedPointFloorOffset(spawnedAvatar, pelvis.position));
Quaternion rotationOffset = Quaternion.Inverse(pelvis.rotation) * spawnedAvatar.pelvis.rotation;
fullBodyCalibration.waist = new Pose(positionOffset, rotationOffset);
_logger.Info("Set waist pose correction " + fullBodyCalibration.waist);
}
if (_trackedDeviceManager.leftFoot.tracked)
{
TrackedDeviceState leftFoot = _trackedDeviceManager.leftFoot;
Vector3 positionOffset = Quaternion.Inverse(spawnedAvatar.leftLeg.rotation) * (spawnedAvatar.leftLeg.position - ApplyTrackedPointFloorOffset(spawnedAvatar, leftFoot.position));
Quaternion rotationOffset = Quaternion.Inverse(leftFoot.rotation) * spawnedAvatar.leftLeg.rotation;
fullBodyCalibration.leftFoot = new Pose(positionOffset, rotationOffset);
_logger.Info("Set left foot pose correction " + fullBodyCalibration.leftFoot);
}
if (_trackedDeviceManager.rightFoot.tracked)
{
TrackedDeviceState rightFoot = _trackedDeviceManager.rightFoot;
Vector3 positionOffset = Quaternion.Inverse(spawnedAvatar.rightLeg.rotation) * (spawnedAvatar.rightLeg.position - ApplyTrackedPointFloorOffset(spawnedAvatar, rightFoot.position));
Quaternion rotationOffset = Quaternion.Inverse(rightFoot.rotation) * spawnedAvatar.rightLeg.rotation;
fullBodyCalibration.rightFoot = new Pose(positionOffset, rotationOffset);
_logger.Info("Set right foot pose correction " + fullBodyCalibration.rightFoot);
}
}
private void OnInputDevicesChanged(TrackedDeviceState state, DeviceUse use)
{
UpdateCalibrationButtons(_avatarManager.currentlySpawnedAvatar.avatar);
}
