428 lines
20 KiB
C#
428 lines
20 KiB
C#
using UnityEngine;
|
|
#if UNITY_EDITOR
|
|
using UnityEditor;
|
|
#endif
|
|
|
|
namespace Passer.Humanoid {
|
|
using Passer.Tracking;
|
|
using Passer.Humanoid.Tracking;
|
|
|
|
public class ArmSensor : HumanoidSensor {
|
|
protected HandTarget handTarget {
|
|
get { return target as HandTarget; }
|
|
}
|
|
|
|
protected HumanoidControl humanoid {
|
|
get { return handTarget.humanoid; }
|
|
}
|
|
|
|
protected new Humanoid.Tracking.ArmSensor sensor;
|
|
|
|
public HandSkeleton handSkeleton;
|
|
|
|
#region Manage
|
|
|
|
protected virtual HandSkeleton FindHandSkeleton(bool isLeft) {
|
|
return null;
|
|
}
|
|
|
|
protected virtual HandSkeleton CreateHandSkeleton(bool isLeft, bool showRealObjects) {
|
|
return null;
|
|
}
|
|
|
|
public virtual void CheckSensor(HandTarget _handTarget) {
|
|
if (handTarget == null)
|
|
target = _handTarget;
|
|
}
|
|
|
|
public virtual void CheckSensor(HandTarget handTarget, HumanoidTracker tracker) {
|
|
if (this.handTarget == null)
|
|
this.target = handTarget;
|
|
if (this.handTarget == null)
|
|
return;
|
|
|
|
if (this.tracker == null)
|
|
this.tracker = tracker;
|
|
|
|
if (enabled && tracker.enabled) {
|
|
if (handSkeleton == null) {
|
|
handSkeleton = FindHandSkeleton(handTarget.isLeft);
|
|
if (handSkeleton == null)
|
|
handSkeleton = CreateHandSkeleton(handTarget.isLeft, handTarget.showRealObjects);
|
|
}
|
|
}
|
|
else {
|
|
#if UNITY_EDITOR
|
|
if (!Application.isPlaying) {
|
|
if (handSkeleton != null)
|
|
Object.DestroyImmediate(handSkeleton.gameObject, true);
|
|
}
|
|
#endif
|
|
handSkeleton = null;
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Start
|
|
|
|
public virtual void Init(HandTarget handTarget) {
|
|
target = handTarget;
|
|
}
|
|
|
|
public override void Start(HumanoidControl _humanoid, Transform targetTransform) {
|
|
base.Start(_humanoid, targetTransform);
|
|
target = targetTransform.GetComponent<HandTarget>();
|
|
}
|
|
|
|
#if UNITY_EDITOR
|
|
public void InitController(SerializedProperty sensorProp, HandTarget handTarget) {
|
|
if (sensorProp == null)
|
|
return;
|
|
|
|
Init(handTarget);
|
|
|
|
SerializedProperty sensorTransformProp = sensorProp.FindPropertyRelative("sensorTransform");
|
|
sensorTransformProp.objectReferenceValue = sensorTransform;
|
|
|
|
SerializedProperty targetProp = sensorProp.FindPropertyRelative("target");
|
|
targetProp.objectReferenceValue = target;
|
|
|
|
if (tracker == null || !tracker.enabled || !enabled)
|
|
return;
|
|
|
|
CheckSensorTransform();
|
|
sensorTransformProp.objectReferenceValue = sensorTransform;
|
|
|
|
ShowSensor(handTarget.humanoid.showRealObjects && handTarget.showRealObjects);
|
|
|
|
SerializedProperty sensor2TargetPositionProp = sensorProp.FindPropertyRelative("sensor2TargetPosition");
|
|
sensor2TargetPositionProp.vector3Value = sensor2TargetPosition;
|
|
SerializedProperty sensor2TargetRotationProp = sensorProp.FindPropertyRelative("sensor2TargetRotation");
|
|
sensor2TargetRotationProp.quaternionValue = sensor2TargetRotation;
|
|
}
|
|
|
|
public void RemoveController(SerializedProperty sensorProp) {
|
|
if (sensorProp == null)
|
|
return;
|
|
|
|
SerializedProperty sensorTransformProp = sensorProp.FindPropertyRelative("sensorTransform");
|
|
sensorTransformProp.objectReferenceValue = null;
|
|
}
|
|
#endif
|
|
|
|
public void CheckSensorTransform(Transform targetTransform, bool isLeft) {
|
|
if (enabled && sensorTransform == null)
|
|
CreateSensorTransform(targetTransform, isLeft);
|
|
else if (!enabled && sensorTransform != null)
|
|
RemoveSensorTransform();
|
|
}
|
|
|
|
public virtual void CreateSensorTransform(Transform targetTransform, bool isLeft) { }
|
|
|
|
protected virtual void CreateSensorTransform(string resourceName, Vector3 sensor2TargetPosition, Quaternion sensor2TargetRotation) {
|
|
CreateSensorTransform(handTarget.hand.target.transform, resourceName, sensor2TargetPosition, sensor2TargetRotation);
|
|
}
|
|
|
|
public override void SetSensor2Target() {
|
|
if (sensorTransform == null || target == null)
|
|
return;
|
|
|
|
sensor2TargetRotation = Quaternion.Inverse(sensorTransform.rotation) * target.transform.rotation;
|
|
sensor2TargetPosition = -target.transform.InverseTransformPoint(sensorTransform.position);
|
|
}
|
|
|
|
public virtual void SetSensor2Target(Vector3 targetPosition, Quaternion targetRotation) {
|
|
if (sensorTransform == null)
|
|
return;
|
|
|
|
sensor2TargetRotation = Quaternion.Inverse(sensorTransform.rotation) * targetRotation;
|
|
//sensor2TargetPosition = -targetTransform.InverseTransformPoint(sensorTransform.position);
|
|
|
|
var worldToLocalMatrix = Matrix4x4.TRS(targetPosition, targetRotation, Vector3.one).inverse;
|
|
sensor2TargetPosition = -worldToLocalMatrix.MultiplyPoint3x4(sensorTransform.position);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Update
|
|
protected void UpdateArm(Humanoid.Tracking.ArmSensor armSensor) {
|
|
float armConfidence = ArmConfidence(armSensor);
|
|
if (handTarget.hand.target.confidence.position > armConfidence)
|
|
UpdateArmIK(armSensor);
|
|
else
|
|
UpdateArmDirect(armSensor);
|
|
}
|
|
|
|
private void UpdateArmDirect(Humanoid.Tracking.ArmSensor armSensor) {
|
|
UpdateShoulder(armSensor);
|
|
UpdateUpperArm(armSensor);
|
|
UpdateForearm(armSensor);
|
|
UpdateHand(armSensor);
|
|
}
|
|
|
|
private void UpdateArmIK(Humanoid.Tracking.ArmSensor armSensor) {
|
|
Vector3 handTargetPosition = handTarget.hand.target.transform.position;
|
|
Quaternion handTargetRotation = handTarget.hand.target.transform.rotation;
|
|
|
|
Vector3 forearmUpAxis = HumanoidTarget.ToQuaternion(armSensor.upperArm.rotation) * Vector3.up;
|
|
if (handTarget.upperArm.target.confidence.rotation < 0.9F) {
|
|
handTarget.upperArm.target.transform.rotation = ArmMovements.UpperArmRotationIK(handTarget.upperArm.target.transform.position, handTargetPosition, forearmUpAxis, handTarget.upperArm.target.length, handTarget.forearm.target.length, handTarget.isLeft);
|
|
handTarget.upperArm.target.confidence = armSensor.upperArm.confidence;
|
|
}
|
|
|
|
if (handTarget.forearm.target.confidence.rotation < 0.9F) {
|
|
handTarget.forearm.target.transform.rotation = ArmMovements.ForearmRotationIK(handTarget.forearm.target.transform.position, handTargetPosition, forearmUpAxis, handTarget.isLeft);
|
|
handTarget.forearm.target.confidence = armSensor.forearm.confidence;
|
|
}
|
|
|
|
handTarget.hand.target.transform.rotation = handTargetRotation;
|
|
handTarget.hand.target.confidence.rotation = armSensor.hand.confidence.rotation;
|
|
}
|
|
|
|
protected void UpdateShoulder(Humanoid.Tracking.ArmSensor armSensor) {
|
|
if (handTarget.shoulder.target.transform == null)
|
|
return;
|
|
|
|
if (armSensor.shoulder.confidence.position > 0)
|
|
handTarget.shoulder.target.transform.position = HumanoidTarget.ToVector3(armSensor.shoulder.position);
|
|
if (armSensor.shoulder.confidence.rotation > 0)
|
|
handTarget.shoulder.target.transform.rotation = HumanoidTarget.ToQuaternion(armSensor.shoulder.rotation);
|
|
handTarget.shoulder.target.confidence = armSensor.upperArm.confidence;
|
|
}
|
|
|
|
protected virtual void UpdateUpperArm(Humanoid.Tracking.ArmSensor armSensor) {
|
|
if (handTarget.upperArm.target.transform != null) {
|
|
if (armSensor.upperArm.confidence.position > 0)
|
|
handTarget.upperArm.target.transform.position = HumanoidTarget.ToVector3(armSensor.upperArm.position);
|
|
else
|
|
handTarget.upperArm.target.transform.position = handTarget.shoulder.target.transform.position + handTarget.shoulder.target.transform.rotation * handTarget.outward * handTarget.shoulder.bone.length;
|
|
|
|
if (armSensor.upperArm.confidence.rotation > 0)
|
|
handTarget.upperArm.target.transform.rotation = HumanoidTarget.ToQuaternion(armSensor.upperArm.rotation);
|
|
|
|
handTarget.upperArm.target.confidence = armSensor.upperArm.confidence;
|
|
}
|
|
}
|
|
|
|
protected virtual void UpdateForearm(Humanoid.Tracking.ArmSensor armSensor) {
|
|
if (handTarget.forearm.target.transform != null) {
|
|
if (armSensor.forearm.confidence.position > 0)
|
|
handTarget.forearm.target.transform.position = HumanoidTarget.ToVector3(armSensor.forearm.position);
|
|
else
|
|
handTarget.forearm.target.transform.position = handTarget.upperArm.target.transform.position + handTarget.upperArm.target.transform.rotation * handTarget.outward * handTarget.upperArm.bone.length;
|
|
|
|
if (armSensor.forearm.confidence.rotation > 0)
|
|
handTarget.forearm.target.transform.rotation = HumanoidTarget.ToQuaternion(armSensor.forearm.rotation);
|
|
|
|
handTarget.forearm.target.confidence = armSensor.forearm.confidence;
|
|
}
|
|
}
|
|
|
|
protected virtual void UpdateHand(Humanoid.Tracking.ArmSensor armSensor) {
|
|
if (handTarget.hand.target.transform != null) {
|
|
if (armSensor.hand.confidence.position > 0 && armSensor.hand.confidence.position >= handTarget.hand.target.confidence.position) {
|
|
handTarget.hand.target.transform.position = HumanoidTarget.ToVector3(armSensor.hand.position);
|
|
handTarget.hand.target.confidence.position = armSensor.hand.confidence.position;
|
|
}
|
|
else if (handTarget.hand.target.confidence.position == 0) // Hmm. I could insert the arm model here when confidence.rotation > 0.5F for example!
|
|
handTarget.hand.target.transform.position = handTarget.forearm.target.transform.position + handTarget.forearm.target.transform.rotation * handTarget.outward * handTarget.forearm.bone.length;
|
|
|
|
if (armSensor.hand.confidence.rotation > 0 && armSensor.hand.confidence.rotation >= handTarget.hand.target.confidence.rotation) {
|
|
handTarget.hand.target.transform.rotation = HumanoidTarget.ToQuaternion(armSensor.hand.rotation);
|
|
handTarget.hand.target.confidence.rotation = armSensor.hand.confidence.rotation;
|
|
}
|
|
}
|
|
}
|
|
protected virtual void UpdateHandTargetTransform(Humanoid.Tracking.ArmSensor armSensor) {
|
|
if (handTarget.hand.target.transform != null) {
|
|
if (armSensor.hand.confidence.rotation > 0 && armSensor.hand.confidence.rotation >= handTarget.hand.target.confidence.rotation) {
|
|
handTarget.hand.target.transform.rotation = sensorTransform.rotation * sensor2TargetRotation;
|
|
handTarget.hand.target.confidence.rotation = armSensor.hand.confidence.rotation;
|
|
}
|
|
if (armSensor.hand.confidence.position > 0 && armSensor.hand.confidence.position >= handTarget.hand.target.confidence.position) {
|
|
handTarget.hand.target.transform.position = sensorTransform.position + handTarget.hand.target.transform.rotation * sensor2TargetPosition;
|
|
handTarget.hand.target.confidence.position = armSensor.hand.confidence.position;
|
|
}
|
|
else if (handTarget.hand.target.confidence.position == 0) // Hmm. I could insert the arm model here when confidence.rotation > 0.5F for example!
|
|
handTarget.hand.target.transform.position = handTarget.forearm.target.transform.position + handTarget.forearm.target.transform.rotation * handTarget.outward * handTarget.forearm.bone.length;
|
|
|
|
}
|
|
}
|
|
|
|
protected virtual void UpdateFingers(Humanoid.Tracking.ArmSensor armSensor) {
|
|
for (int i = 0; i < (int)Humanoid.Tracking.Finger.Count; i++) {
|
|
UpdateFinger(armSensor.fingers[i], i);
|
|
}
|
|
}
|
|
|
|
private void UpdateFinger(Humanoid.Tracking.ArmSensor.Finger fingerSensor, int i) {
|
|
Transform proximalTarget = handTarget.fingers.allFingers[i].proximal.target.transform;
|
|
proximalTarget.rotation = proximalTarget.parent.rotation * HumanoidTarget.ToQuaternion(fingerSensor.proximal.rotation);
|
|
|
|
Transform intermediateTarget = handTarget.fingers.allFingers[i].intermediate.target.transform;
|
|
intermediateTarget.rotation = intermediateTarget.parent.rotation * HumanoidTarget.ToQuaternion(fingerSensor.intermediate.rotation);
|
|
|
|
Transform distalTarget = handTarget.fingers.allFingers[i].distal.target.transform;
|
|
distalTarget.rotation = distalTarget.parent.rotation * HumanoidTarget.ToQuaternion(fingerSensor.distal.rotation);
|
|
|
|
handTarget.DetermineFingerCurl((Humanoid.Tracking.Finger)i);
|
|
}
|
|
|
|
#region Skeleton
|
|
|
|
protected virtual void UpdateHandFromSkeleton() {
|
|
Transform wristBone = handSkeleton.GetWristBone();
|
|
handTarget.hand.target.transform.position = wristBone.transform.position;
|
|
handTarget.hand.target.transform.rotation = wristBone.transform.rotation;
|
|
handTarget.hand.target.confidence.position = handSkeleton.positionConfidence;
|
|
handTarget.hand.target.confidence.rotation = handSkeleton.rotationConfidence;
|
|
|
|
UpdateThumbFromSkeleton();
|
|
UpdateIndexFingerFromSkeleton();
|
|
UpdateMiddleFingerFromSkeleton();
|
|
UpdateRingFingerFromSkeleton();
|
|
UpdateLittleFingerFromSkeleton();
|
|
|
|
}
|
|
|
|
protected virtual void UpdateThumbFromSkeleton() {
|
|
FingersTarget.TargetedFinger finger = handTarget.fingers.thumb;
|
|
UpdateFingerBoneFromSkeleton(finger.proximal.target.transform, Finger.Thumb, FingerBone.Proximal);
|
|
UpdateFingerBoneFromSkeleton(finger.intermediate.target.transform, Finger.Thumb, FingerBone.Intermediate);
|
|
UpdateFingerBoneFromSkeleton(finger.distal.target.transform, Finger.Thumb, FingerBone.Distal);
|
|
}
|
|
|
|
protected void UpdateIndexFingerFromSkeleton() {
|
|
FingersTarget.TargetedFinger finger = handTarget.fingers.index;
|
|
UpdateFingerBoneFromSkeleton(finger.proximal.target.transform, Finger.Index, FingerBone.Proximal);
|
|
UpdateFingerBoneFromSkeleton(finger.intermediate.target.transform, Finger.Index, FingerBone.Intermediate);
|
|
UpdateFingerBoneFromSkeleton(finger.distal.target.transform, Finger.Index, FingerBone.Distal);
|
|
}
|
|
|
|
protected void UpdateMiddleFingerFromSkeleton() {
|
|
FingersTarget.TargetedFinger finger = handTarget.fingers.middle;
|
|
UpdateFingerBoneFromSkeleton(finger.proximal.target.transform, Finger.Middle, FingerBone.Proximal);
|
|
UpdateFingerBoneFromSkeleton(finger.intermediate.target.transform, Finger.Middle, FingerBone.Intermediate);
|
|
UpdateFingerBoneFromSkeleton(finger.distal.target.transform, Finger.Middle, FingerBone.Distal);
|
|
}
|
|
|
|
protected void UpdateRingFingerFromSkeleton() {
|
|
FingersTarget.TargetedFinger finger = handTarget.fingers.ring;
|
|
UpdateFingerBoneFromSkeleton(finger.proximal.target.transform, Finger.Ring, FingerBone.Proximal);
|
|
UpdateFingerBoneFromSkeleton(finger.intermediate.target.transform, Finger.Ring, FingerBone.Intermediate);
|
|
UpdateFingerBoneFromSkeleton(finger.distal.target.transform, Finger.Ring, FingerBone.Distal);
|
|
}
|
|
|
|
protected void UpdateLittleFingerFromSkeleton() {
|
|
FingersTarget.TargetedFinger finger = handTarget.fingers.little;
|
|
UpdateFingerBoneFromSkeleton(finger.proximal.target.transform, Finger.Little, FingerBone.Proximal);
|
|
UpdateFingerBoneFromSkeleton(finger.intermediate.target.transform, Finger.Little, FingerBone.Intermediate);
|
|
UpdateFingerBoneFromSkeleton(finger.distal.target.transform, Finger.Little, FingerBone.Distal);
|
|
}
|
|
|
|
protected virtual void UpdateFingerBoneFromSkeleton(Transform targetTransform, Finger finger, FingerBone fingerBone) {
|
|
if (handSkeleton == null)
|
|
return;
|
|
|
|
Transform thisBoneTransform = handSkeleton.GetBone(finger, fingerBone);
|
|
Transform nextBoneTransform = handSkeleton.GetBone(finger, fingerBone + 1);
|
|
if (thisBoneTransform == null || nextBoneTransform == null)
|
|
return;
|
|
|
|
Vector3 direction = nextBoneTransform.position - thisBoneTransform.position;
|
|
if (handTarget.isLeft)
|
|
targetTransform.rotation = Quaternion.LookRotation(direction, handTarget.hand.target.transform.forward) * Quaternion.Euler(-90, 0, 90);
|
|
else
|
|
targetTransform.rotation = Quaternion.LookRotation(direction, handTarget.hand.target.transform.forward) * Quaternion.Euler(-90, 0, -90);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#endregion
|
|
|
|
public float ArmConfidence(Humanoid.Tracking.ArmSensor armSensor) {
|
|
float armOrientationsConfidence =
|
|
//armSensor.shoulder.confidence.rotation *
|
|
armSensor.upperArm.confidence.rotation *
|
|
armSensor.forearm.confidence.rotation;
|
|
return armOrientationsConfidence;
|
|
}
|
|
|
|
|
|
public virtual void Vibrate(float length, float strength) {
|
|
}
|
|
}
|
|
}
|
|
|
|
namespace Passer.Humanoid {
|
|
public class ArmController : Humanoid.ArmSensor {
|
|
protected Humanoid.Tracking.Sensor.ID sensorID;
|
|
protected Controller controllerInput;
|
|
public Humanoid.Tracking.ArmController oldController;
|
|
public Passer.Tracking.ControllerComponent controller;
|
|
|
|
public override Tracker.Status status {
|
|
get {
|
|
if (oldController == null)
|
|
return Tracker.Status.Unavailable;
|
|
else
|
|
return oldController.status;
|
|
}
|
|
set {
|
|
if (oldController != null)
|
|
oldController.status = value;
|
|
}
|
|
}
|
|
|
|
#region Start
|
|
public override void Start(HumanoidControl _humanoid, Transform targetTransform) {
|
|
base.Start(_humanoid, targetTransform);
|
|
sensorID = handTarget.isLeft ? Humanoid.Tracking.Sensor.ID.LeftHand : Humanoid.Tracking.Sensor.ID.RightHand;
|
|
controllerInput = Controllers.GetController(0);
|
|
}
|
|
#endregion
|
|
|
|
#region Update
|
|
public override void Update() {
|
|
if (tracker == null || !tracker.enabled || !enabled)
|
|
return;
|
|
|
|
oldController.Update();
|
|
if (oldController.status != Tracker.Status.Tracking)
|
|
return;
|
|
|
|
UpdateSensorTransform(oldController);
|
|
UpdateTargetTransform();
|
|
}
|
|
|
|
protected void UpdateInput(Controller controller, Humanoid.Tracking.ArmController armController) {
|
|
if (handTarget.isLeft)
|
|
SetControllerInput(controller.left, armController);
|
|
else
|
|
SetControllerInput(controller.right, armController);
|
|
}
|
|
|
|
protected void SetControllerInput(ControllerSide controllerSide, Humanoid.Tracking.ArmController armController) {
|
|
controllerSide.stickHorizontal += armController.input.stickHorizontal;
|
|
controllerSide.stickVertical += armController.input.stickVertical;
|
|
controllerSide.stickButton |= armController.input.stickPress;
|
|
|
|
//controllerSide.up |= armController.input.up;
|
|
//controllerSide.down |= armController.input.down;
|
|
//controllerSide.left |= armController.input.left;
|
|
//controllerSide.right |= armController.input.right;
|
|
|
|
controllerSide.buttons[0] |= armController.input.buttons[0];
|
|
controllerSide.buttons[1] |= armController.input.buttons[1];
|
|
controllerSide.buttons[2] |= armController.input.buttons[2];
|
|
controllerSide.buttons[3] |= armController.input.buttons[3];
|
|
|
|
controllerSide.trigger1 += armController.input.trigger1;
|
|
controllerSide.trigger2 += armController.input.trigger2;
|
|
|
|
controllerSide.option |= armController.input.option;
|
|
}
|
|
#endregion
|
|
}
|
|
} |