void ProcessSkeleton(Skeleton skeleton)
{
print($"Frame Counter: {frameCtr}");
//Calculate the model position: take the Torso position and invert movement along the Z axis
UnityEngine.Vector3 torsoPos = Quaternion.Euler(0f, 180f, 0f) * (-0.001f * skeleton.GetJoint(JointType.Torso).ToVector3());
torsoPos[0] = torsoPos[0] * posModifier[0];
for (int i = 0; i < 3; i++)
{
torsoPos[i] = torsoPos[i] + posModifier[i];
}
torsoPos = torsoPos + posModifierPreview;
transform.position = torsoPos;
foreach (var riggedJoint in jointsRigged)
{
try {
//Get joint from the Nuitrack
nuitrack.Joint joint = skeleton.GetJoint(riggedJoint.Key);
ModelJoint modelJoint = riggedJoint.Value;
//Calculate the model bone rotation: take the mirrored joint orientation, add a basic rotation of the model bone, invert movement along the Z axis
Quaternion jointOrient = Quaternion.Inverse(CalibrationInfo.SensorOrientation) * (joint.ToQuaternion()) * modelJoint.baseRotOffset;
modelJoint.bone.rotation = jointOrient;
} catch {
// Ignore IndexOutOfRangeException (it occurs because model has more available joints than those provided by orbbec)
}
}
}
/// <summary>
/// Quaternion で渡された姿勢のうち、X, Y, Z 軸いずれか周り成分を抽出してサーボ角に反映します
/// </summary>
/// <param name="rot">目標姿勢</param>
/// <param name="joint">指定サーボ</param>
/// <returns>回転させた軸成分を除いた残りの回転 Quaternion</returns>
Quaternion ApplyDirectRotation(Quaternion rot, float angle, ModelJoint joint)
{
Quaternion q;
Vector3 axis = Vector3.right;
float direction = (joint.isInverse ? -1f : 1f); // 逆転なら-1
switch (joint.targetAxis)
{
case ModelJoint.Axis.X:
axis = Vector3.right;
break;
case ModelJoint.Axis.Y:
axis = Vector3.up;
break;
case ModelJoint.Axis.Z:
axis = Vector3.forward;
break;
}
var actualAngle = joint.SetServoValue(angle * direction);
//q = Quaternion.AngleAxis(angle, axis);
q = Quaternion.AngleAxis(actualAngle / direction, axis); // 角度制限を考慮
return(Quaternion.Inverse(q) * rot);
}
/// <summary>
/// Getting skeleton data from thr sensor and updating transforms of the model bones
/// </summary>
void ProcessSkeleton(nuitrack.Skeleton skeleton)
{
foreach (var riggedJoint in jointsRigged)
{
//Get joint from the Nuitrack
nuitrack.Joint joint = skeleton.GetJoint(riggedJoint.Key);
//Get modelJoint
ModelJoint modelJoint = riggedJoint.Value;
//Bone position
Vector3 newPos = Quaternion.Euler(0f, 180f, 0f) * (0.001f * joint.ToVector3());
modelJoint.bone.position = newPos;
//Bone rotation
Quaternion jointOrient = Quaternion.Inverse(CalibrationInfo.SensorOrientation) * (joint.ToQuaternionMirrored()) * modelJoint.baseRotOffset;
modelJoint.bone.rotation = jointOrient;
//Bone scale
if (modelJoint.parentBone != null)
{
//Take the Transform of a parent bone
Transform parentBone = modelJoint.parentBone;
//calculate how many times the distance between the child bone and its parent bone has changed compared to the base distance (which was recorded at the start)
float scaleDif = modelJoint.baseDistanceToParent / Vector3.Distance(newPos, parentBone.position);
//change the size of the bone to the resulting value (On default bone size (1,1,1))
parentBone.localScale = Vector3.one / scaleDif;
}
}
}
private void RefreshJointRotation(nuitrack.Skeleton skeleton)
{
foreach (var riggedJoint in jointsRigged)
{
nuitrack.Joint joint = skeleton.GetJoint(riggedJoint.Key);
ModelJoint modelJoint = riggedJoint.Value;
Quaternion jointOrient = Quaternion.Inverse(CalibrationInfo.SensorOrientation) * (joint.ToQuaternion()) * modelJoint.baseRotOffset;
modelJoint.bone.rotation = jointOrient;
}
}
void SetJoint(Transform tr, JointType jointType)
{
ModelJoint modelJoint = new ModelJoint()
{
bone = tr,
jointType = jointType
};
modelJoints.Add(modelJoint);
}
// Update is called once per frame
void Update()
{
if (NuitrackManager.NumUsers > 0)
{
if (timer <= waitTime)
{
timer += Time.deltaTime;
BoneScalingProgress.text = "Bone Scaling: In Progress...";
}
else if (timer >= waitTime)
{
BoneScalingProgress.text = "Bone Scaling: Completed";
}
// get ID of user
nuitrack.User[] users = NuitrackManager.Users;
nuitrack.User CurrentUser = users[0];
int CurrentUserID = CurrentUser.ID;
foreach (var riggedJoint in jointsRigged)
{
//Get joint from the Nuitrack
nuitrack.Joint joint = NuitrackManager.SkeletonData.GetSkeletonByID(CurrentUserID).GetJoint(riggedJoint.Key);
if (joint.Confidence > 0.5) //Currently, there are only two values of confidence: 0 (Nuitrack thinks that this isn't a joint) and 0.75 (a joint).
{
ModelJoint modeljoint = riggedJoint.Value; //get modelJoint
nuitrack.Joint parentjoint = NuitrackManager.SkeletonData.GetSkeletonByID(CurrentUserID).GetJoint(modeljoint.parentJointType);
Vector3 newPos = 0.001f * joint.ToVector3(); //given in mm
Vector3 parentPos = 0.001f * parentjoint.ToVector3();
// Coinvert nuitrack joint orientation to quaternion
Quaternion jointOrient = joint.ToQuaternion();
// Update Model joint to tracked orientation
modeljoint.bone.rotation = jointOrient * modeljoint.baseRotOffset;
// perform bone scaling for 5 seconds at the start maybe?
if (modeljoint.parentBone != null && timer <= waitTime)
{
Debug.Log("BONE SCALING PERFORMED...........");
// take the transform of the parent bone
//Transform parentBone = modeljoint.parentBone;
// calculate how many times the distance between the child bone and its parent bone has changed compared to the base distances (which was recorded at the start)
float scaleDif = modeljoint.baseDistanceToParent / Vector3.Distance(newPos, parentPos);
// change the size of the bone to the resulting value
modeljoint.parentBone.localScale = Vector3.one / scaleDif;
}
}
}
}
}
/// <summary>
/// Quaternion で渡された姿勢のうち、X, Y, Z 軸いずれか周り成分を抽出してサーボ角に反映します
/// </summary>
/// <param name="rot">目標姿勢</param>
/// <param name="joint">指定サーボ</param>
/// <returns>回転させた軸成分を除いた残りの回転 Quaternion</returns>
Quaternion ApplyPartialRotation(Quaternion rot, ModelJoint joint)
{
Quaternion q = rot;
Vector3 axis = Vector3.right;
float direction = (joint.isInverse ? -1f : 1f); // 逆転なら-1
switch (joint.targetAxis)
{
case ModelJoint.Axis.X:
q.y = q.z = 0;
if (q.x < 0)
{
direction = -direction;
}
axis = Vector3.right;
break;
case ModelJoint.Axis.Y:
q.x = q.z = 0;
if (q.y < 0)
{
direction = -direction;
}
axis = Vector3.up;
break;
case ModelJoint.Axis.Z:
q.x = q.y = 0;
if (q.z < 0)
{
direction = -direction;
}
axis = Vector3.forward;
break;
}
if (q.w == 0 && q.x == 0 && q.y == 0 && q.z == 0)
{
Debug.Log("Joint: " + joint.name + " rotation N/A");
q = Quaternion.identity;
}
q.Normalize();
float angle = Mathf.Acos(q.w) * 2.0f * Mathf.Rad2Deg * direction;
var actualAngle = joint.SetServoValue(angle);
return(rot * Quaternion.Inverse(q));
//return rot * Quaternion.Inverse(Quaternion.AngleAxis(actualAngle, axis);
}
// Update is called once per frame
void Update()
{
if (controller)
{
foreach (var servo in controller.Servos)
{
string servoId = servo.GetServoIdString();
ModelJoint joint = GetJointById(servoId);
if (joint != null)
{
joint.SetServoPosition(servo.GetServoValue());
}
}
}
}
void ProcessSkeleton(nuitrack.Skeleton skeleton)
{
//Vector3 torsoPos = Quaternion.Euler(0f, 0f, 0f) * (0.001f * skeleton.GetJoint(nuitrack.JointType.Torso).ToVector3());
//transform.position = torsoPos;
foreach (var riggedJoint in jointsRigged)
{
nuitrack.Joint joint = skeleton.GetJoint(riggedJoint.Key);
ModelJoint modelJoint = riggedJoint.Value;
Quaternion jointOrient =
Quaternion.Inverse(CalibrationInfo.SensorOrientation)
* (joint.ToQuaternionMirrored())
* modelJoint.baseRotOffset;
modelJoint.bone.rotation = jointOrient;
}
}
/// <summary>
/// Getting skeleton data from sensor and update model bones transforms
/// </summary>
void ProcessSkeleton(nuitrack.Skeleton skeleton)
{
if (skeleton == null)
{
return;
}
if (!firstOffset)
{
firstOffset = true;
StartCoroutine(CalculateOffset());
}
foreach (var riggedJoint in jointsRigged)
{
nuitrack.Joint j = skeleton.GetJoint(riggedJoint.Key);
if (j.Confidence > 0.5f)
{
//Bone position
Vector3 newPos = (q180) * (Vector3.up * CalibrationInfo.FloorHeight + CalibrationInfo.SensorOrientation * (0.001f * j.ToVector3())) * scale + basePivotOffset;
ModelJoint rj = riggedJoint.Value;
//Bone scale
if (rj.parentBone != null)
{
Transform bone = rj.parentBone;
bone.parent = bone.root;
float scaleDif = rj.baseDistanceToParent / Vector3.Distance(newPos, bone.position);
bone.localScale = Vector3.one / scaleDif;
}
rj.bone.position = newPos;
if (j.Type != nuitrack.JointType.None)
{
Quaternion jointOrient = CalibrationInfo.SensorOrientation * (j.ToQuaternionMirrored());
rj.bone.rotation = q0 * Quaternion.Inverse(CalibrationInfo.SensorOrientation) * jointOrient * rj.baseRotOffset;
}
}
}
leftHandPos = jointsRigged[nuitrack.JointType.LeftWrist].bone.position;
rightHandPos = jointsRigged[nuitrack.JointType.RightWrist].bone.position;
}
/// <summary>
/// Getting skeleton data from thr sensor and updating transforms of the model bones
/// </summary>
void Process(UserData user)
{
if (!alignmentBoneLength)
{
jointsRigged[rootJoint].bone.position = GetJointLocalPos(GetJoint(rootJoint).Position);
}
foreach (var riggedJoint in jointsRigged)
{
//Get joint from the Nuitrack
//nuitrack.Joint joint = skeleton.GetJoint(riggedJoint.Key);
UserData.SkeletonData.Joint jointTransform = user.Skeleton.GetJoint(riggedJoint.Key);
if (jointTransform.Confidence > JointConfidence)
{
//Get modelJoint
ModelJoint modelJoint = riggedJoint.Value;
//Bone rotation
Quaternion jointRotation = IsTransformSpace ? jointTransform.RotationMirrored : jointTransform.Rotation;
modelJoint.bone.rotation = SpaceTransform.rotation * (jointRotation * modelJoint.baseRotOffset);
if (alignmentBoneLength)
{
Vector3 newPos = GetJointLocalPos(jointTransform.Position);
modelJoint.bone.position = newPos;
//Bone scale
if (modelJoint.parentBone != null && modelJoint.jointType.GetParent() != rootJoint)
{
//Take the Transform of a parent bone
Transform parentBone = modelJoint.parentBone;
//calculate how many times the distance between the child bone and its parent bone has changed compared to the base distance (which was recorded at the start)
float scaleDif = modelJoint.baseDistanceToParent / Vector3.Distance(newPos, parentBone.position);
//change the size of the bone to the resulting value (On default bone size (1,1,1))
parentBone.localScale = Vector3.one / scaleDif;
//compensation for size due to hierarchy
parentBone.localScale *= parentBone.localScale.x / parentBone.lossyScale.x;
}
}
}
}
}
void ProcessSkeleton(nuitrack.Skeleton skeleton)
{
//Calculate the model position: take the Torso position and invert movement along the Z axis
Vector3 torsoPos = Quaternion.Euler(0f, 180f, 0f) * (0.001f * skeleton.GetJoint(nuitrack.JointType.Torso).ToVector3());
transform.position = torsoPos + basePivotOffset;
foreach (var riggedJoint in jointsRigged)
{
//Get joint from the Nuitrack
nuitrack.Joint joint = skeleton.GetJoint(riggedJoint.Key);
ModelJoint modelJoint = riggedJoint.Value;
//Calculate the model bone rotation: take the mirrored joint orientation, add a basic rotation of the model bone, invert movement along the Z axis
Quaternion jointOrient = Quaternion.Inverse(CalibrationInfo.SensorOrientation) * (joint.ToQuaternionMirrored()) * modelJoint.baseRotOffset;
modelJoint.bone.rotation = jointOrient;
}
}
/// <summary>
/// サーボ情報一つ分を記憶
/// </summary>
/// <param name="servoNo"></param>
void AddServo(ServoPosition servoNo)
{
var joint = ModelJoint.GetJointById((int)servoNo, ref _joints);
if (joint)
{
servos.Add(servoNo, joint);
// 開発用に、いったん制限なしとする
joint.minAngle = -179f;
joint.maxAngle = 180f;
if (joint.minAngle > 0f)
{
joint.minAngle = 0f; // 開発用。Tポーズだと上腕が0degのため
}
if (joint.maxAngle < 0f)
{
joint.maxAngle = 0f; // 開発用。Tポーズだと上腕が0degのため
}
}
//Debug.Log(servoId + " " + joint.name);
}
// Start is called before the first frame update
void Start()
{
// 速度をゆっくりに強制
ModelJoint.SetAllJointsMaxSpeed(60f);
_controller = GetComponent <PreMaid.RemoteController.PreMaidController>();
List <TMP_Dropdown.OptionData> serialPortNamesList = new List <TMP_Dropdown.OptionData>();
var portNames = SerialPort.GetPortNames();
foreach (var VARIABLE in portNames)
{
TMP_Dropdown.OptionData optionData = new TMP_Dropdown.OptionData(VARIABLE);
serialPortNamesList.Add(optionData);
Debug.Log(VARIABLE);
}
_serialPortsDropdown.ClearOptions();
_serialPortsDropdown.AddOptions(serialPortNamesList);
_joints = target.GetComponentsInChildren <ModelJoint>();
}
// Start is called before the first frame update
void Start()
{
_controller = GetComponent <PreMaid.RemoteController.PreMaidControllerSPUP>();
List <TMP_Dropdown.OptionData> serialPortNamesList = new List <TMP_Dropdown.OptionData>();
var portNames = SerialPortUtility.SerialPortUtilityPro.GetConnectedDeviceList(SerialPortUtility.SerialPortUtilityPro.OpenSystem.BluetoothSSP);
if (portNames != null)
{
foreach (var VARIABLE in portNames)
{
TMP_Dropdown.OptionData optionData = new TMP_Dropdown.OptionData(VARIABLE.SerialNumber);
serialPortNamesList.Add(optionData);
Debug.Log(VARIABLE);
}
_serialPortsDropdown.ClearOptions();
_serialPortsDropdown.AddOptions(serialPortNamesList);
}
else
{
// Android実機でのデバッグ用
serialPortNamesList.Add(new TMP_Dropdown.OptionData("RNBT-4FFA"));
serialPortNamesList.Add(new TMP_Dropdown.OptionData("RNBT-50D6"));
serialPortNamesList.Add(new TMP_Dropdown.OptionData("RNBT-9C50"));
serialPortNamesList.Add(new TMP_Dropdown.OptionData("RNBT-94F6"));
_serialPortsDropdown.ClearOptions();
_serialPortsDropdown.AddOptions(serialPortNamesList);
_serialPortsDropdown.SetValueWithoutNotify(0);
}
// 上半身だけを操作対象とする
_controller.jointMask = (uint)(PreMaidControllerSPUP.JointMask.UpperBody);
// 関節速度制限
ModelJoint.SetAllJointsMaxSpeed(90f);
//対象のAnimatorにBoneにHumanoidModelJoint.csのアタッチ漏れがあるかもしれない
//なので、一旦全部検索して、見つからなかったサーボ情報はspineに全部動的にアタッチする
Transform spineBone = target.GetBoneTransform(HumanBodyBones.Spine);
//仮でspineにでも付けておこう
if (target != null)
{
var joints = target.GetComponentsInChildren <HumanoidModelJoint>();
foreach (PreMaidServo.ServoPosition item in Enum.GetValues(typeof(PreMaidServo.ServoPosition)))
{
if (Array.FindIndex(joints, joint => joint.TargetServo == item) == -1)
{
var jointScript = spineBone.gameObject.AddComponent <HumanoidModelJoint>();
jointScript.TargetServo = item;
}
}
// 手首だけは最高速度を高くしておく
var modeljoints = target.GetComponentsInChildren <ModelJoint>();
foreach (ModelJoint j in modeljoints)
{
if (j.ServoID == "15" || j.ServoID == "17")
{
j.maxSpeed = 180f;
}
}
}
_joints = target.GetComponentsInChildren <HumanoidModelJoint>();
}
