private void FusedUpdatArm()
{
// save hand pos/rot to restore after IK.
Vector3 externalHandTargetPosition = handTarget.hand.target.transform.position;
Quaternion externalHandTargetRotation = handTarget.hand.target.transform.rotation;
//UpdateShoulder(handTarget.shoulder.target);
// I am not happy with the Neuron shoulder rotation in combination with IK,
// Probably it is better to use the IK rotation
// But for now, the shoulders are fixed
Quaternion neuronUpperArmRotation = tracker.trackerTransform.rotation * upperArmSensor.rotation;
Vector3 elbowAxis = neuronUpperArmRotation * Vector3.up;
Quaternion upperArmRotation = ArmMovements.UpperArmRotationIK(
handTarget.upperArm.target.transform.position, externalHandTargetPosition, elbowAxis, handTarget.upperArm.target.length, handTarget.forearm.target.length, handTarget.isLeft);
upperArmBias = CalculateBias(upperArmBias, tracker.trackerTransform.rotation * upperArmSensor.rotation, upperArmRotation);
UpdateUpperArm(handTarget.upperArm.target);
Quaternion forearmRotation = ArmMovements.ForearmRotationIK(
handTarget.forearm.target.transform.position, externalHandTargetPosition, elbowAxis, handTarget.isLeft);
forearmBias = CalculateBias(forearmBias, tracker.trackerTransform.rotation * forearmSensor.rotation, forearmRotation);
UpdateForearm(handTarget.forearm.target);
Debug.DrawRay(handTarget.forearm.target.transform.position, elbowAxis, Color.magenta);
if (handTarget.hand.target.confidence.rotation < handSensor.rotationConfidence)
{
handBias = Quaternion.identity;
}
else
{
handBias = CalculateBias(handBias, tracker.trackerTransform.rotation * handSensor.rotation, externalHandTargetRotation);
}
UpdateHand(handTarget.hand.target);
}
public Leg(ArmMovements.BodySide bodySide_in, Animator animator, Transform characterTransform_in) {
characterTransform = characterTransform_in;
if (bodySide_in == ArmMovements.BodySide.Left) {
upperLeg = animator.GetBoneTransform(HumanBodyBones.LeftUpperLeg);
lowerLeg = animator.GetBoneTransform(HumanBodyBones.LeftLowerLeg);
foot = animator.GetBoneTransform(HumanBodyBones.LeftFoot);
} else {
upperLeg = animator.GetBoneTransform(HumanBodyBones.RightUpperLeg);
lowerLeg = animator.GetBoneTransform(HumanBodyBones.RightLowerLeg);
foot = animator.GetBoneTransform(HumanBodyBones.RightFoot);
}
fromNormUpperLeg = Quaternion.Inverse(Quaternion.LookRotation(foot.position - upperLeg.position, characterTransform.forward)) * upperLeg.rotation;
fromNormLowerLeg = Quaternion.Inverse(Quaternion.LookRotation(foot.position - lowerLeg.position, characterTransform.forward)) * lowerLeg.rotation;
fromNormFoot = Quaternion.Inverse(Quaternion.LookRotation(characterTransform.forward)) * foot.rotation;
upperLegLength = Vector3.Distance(upperLeg.position, lowerLeg.position);
lowerLegLength = Vector3.Distance(lowerLeg.position, foot.position);
upperLegLength2 = upperLegLength * upperLegLength;
lowerLegLength2 = lowerLegLength * lowerLegLength;
}
public void StartMovements() {
ivr = this.GetComponent<InstantVR>();
headTarget = ivr.headTarget;
leftHandTarget = ivr.leftHandTarget;
rightHandTarget = ivr.rightHandTarget;
hipTarget = ivr.hipTarget;
leftFootTarget = ivr.leftFootTarget;
rightFootTarget = ivr.rightFootTarget;
animator = ivr.transform.GetComponentInChildren<Animator>();
if (animator == null) {
StopMovements();
} else {
characterRigidbody = animator.GetComponent<Rigidbody>();
if (characterRigidbody != null)
characterTransform = characterRigidbody.GetComponent<Transform>();
if (leftArm == null)
leftArm = new ArmMovements(ivr, ArmMovements.BodySide.Left, this);
leftArm.Initialize(ivr, ArmMovements.BodySide.Left, this);
if (rightArm == null)
rightArm = new ArmMovements(ivr, ArmMovements.BodySide.Right, this);
rightArm.Initialize(ivr, ArmMovements.BodySide.Right, this);
Transform neck, spine, hips;
neck = animator.GetBoneTransform(HumanBodyBones.Neck);
if (neck == null)
neck = animator.GetBoneTransform(HumanBodyBones.Head);
spine = animator.GetBoneTransform(HumanBodyBones.Spine);
hips = animator.GetBoneTransform(HumanBodyBones.Hips);
torso = new Torso(neck, spine, hips, hipTarget, headTarget, rightArm);
leftLeg = new Leg(ArmMovements.BodySide.Left, animator, hipTarget);
rightLeg = new Leg(ArmMovements.BodySide.Right, animator, hipTarget);
if (rightHandTarget != null) {
rightHandOTarget = rightHandTarget;
/*
float targetScaleZ = rightHandTarget.transform.localScale.z;
rightPalmTarget = new GameObject();
rightPalmTarget.name = "Palm_R_Target";
rightPalmTarget.transform.parent = rightHandTarget.transform;
rightPalmTarget.transform.localPosition = new Vector3(0, 0, -palmLength/targetScaleZ);
rightPalmTarget.transform.localEulerAngles = Vector3.zero;
rightHandTarget = rightPalmTarget.transform;
*/
}
if (leftHandTarget != null) {
leftHandOTarget = leftHandTarget;
/*
float targetScaleZ = leftHandTarget.transform.localScale.z;
leftPalmTarget = new GameObject();
leftPalmTarget.name = "Palm_L_Target";
leftPalmTarget.transform.parent = leftHandTarget.transform;
leftPalmTarget.transform.localPosition = new Vector3(0, 0, -palmLength/targetScaleZ);
leftPalmTarget.transform.localEulerAngles = Vector3.zero;
leftHandTarget = leftPalmTarget.transform;
*/
}
if (headTarget == null && enableTorso) {
GameObject neckTargetGO = new GameObject("Neck_Target");
headTarget = neckTargetGO.transform;
headTarget.parent = characterTransform;
headTarget.position = torso.neck.position;
headTarget.rotation = characterTransform.rotation;
}
if (enableLegs) {
if (rightFootTarget == null) {
//userLegTargets = false;
GameObject rightFootTargetGO = new GameObject("Foot_R_Target");
rightFootTarget = rightFootTargetGO.transform;
rightFootTarget.parent = characterTransform;
rightFootTarget.position = rightLeg.foot.position;
rightFootTarget.rotation = characterTransform.rotation;
}
if (leftFootTarget == null) {
//userLegTargets = false;
GameObject leftFootTargetGO = new GameObject("Foot_L_Target");
leftFootTarget = leftFootTargetGO.transform;
leftFootTarget.parent = characterTransform;
leftFootTarget.position = leftLeg.foot.position;
leftFootTarget.rotation = characterTransform.rotation;
}
}
if (IsInTPose(leftArm, rightArm)) {
CalculateFromNormTPose(leftArm, rightArm);
} else {
CalculateFromNormTracking(leftArm, rightArm, leftHandTarget, rightHandTarget);
}
}
}
public bool AutoVertical(Transform rightHandOTarget, Transform rightHandTarget, ArmMovements rightArm, Transform leftHandOTarget, Transform leftHandTarget, ArmMovements leftArm, Transform neckTarget) {
Vector3 neckDelta = Vector3.zero;
Vector3 leftToTarget = leftHandOTarget.position - leftArm.upperArmStartPosition;
Vector3 rightToTarget = rightHandOTarget.position - rightArm.upperArmStartPosition;
float leftOver = leftToTarget.magnitude - leftArm.length;
float rightOver = rightToTarget.magnitude - rightArm.length;
if (leftOver > 0) {
if (rightOver > leftOver)
neckDelta = rightToTarget.normalized * rightOver;
else
neckDelta = leftToTarget.normalized * leftOver;
} else if (rightOver > 0)
neckDelta = rightToTarget.normalized * rightOver;
neckTarget.position = neckStartPosition + new Vector3(0, neckDelta.y, 0);
float dY = neckTarget.transform.position.y - neck.position.y;
if (hips.position.y + dY < hipStartPosition.y) {
hips.Translate(0, dY, 0, Space.World);
} else if (hips.position.y + dY > hipStartPosition.y) {
hips.position = new Vector3(hips.position.x, hipStartPosition.y, hips.position.z);
}
rightArm.Calculate(rightHandTarget);
leftArm.Calculate(leftHandTarget);
return (hips.position.y < hipStartPosition.y);
}
public float BendAngle(Vector3 torsoTarget, ArmMovements arm) {
float baseAngle = Vector3.Angle (spineStartOrientation, torsoTarget - spine.position);
float dSpine2Target = Vector3.Distance (spine.position, torsoTarget);
float spineAngle = Mathf.Acos((dSpine2Target * dSpine2Target + length*length - arm.length * arm.length)/ (2 * dSpine2Target * length)) * Mathf.Rad2Deg;
if (float.IsNaN(spineAngle)) spineAngle = 0;
return Mathf.Min(baseAngle - spineAngle, IVR_BodyMovements.maxHipAngle);
}
public float AutoHorizontal(Transform rightHandOTarget, Transform rightHandTarget, ArmMovements rightArm, Transform leftHandOTarget, Transform leftHandTarget, ArmMovements leftArm, Transform neckTarget) {
float bendAngle = 0;
Vector3 torsoTarget = Vector3.zero;
Vector3 dShoulderNeck = (leftArm.upperArm.position - rightArm.upperArm.position) / 2;
Vector3 leftToTarget = leftHandOTarget.position - leftArm.upperArmStartPosition;
Vector3 rightToTarget = rightHandOTarget.position - rightArm.upperArmStartPosition;
float leftOver = leftToTarget.magnitude - leftArm.length;
float rightOver = rightToTarget.magnitude - rightArm.length;
if (leftOver > 0) {
if (rightOver > 0) {
Vector3 torsoTargetR = rightHandOTarget.position + dShoulderNeck;
Vector3 torsoTargetL = leftHandOTarget.position - dShoulderNeck;
float bendAngleR = BendAngle(torsoTargetR, rightArm);
float bendAngleL = BendAngle(torsoTargetL, leftArm);
if (bendAngleR > bendAngleL)
torsoTarget = torsoTargetR;
else
torsoTarget = torsoTargetL;
} else
torsoTarget = leftHandOTarget.position - dShoulderNeck;
} else if (rightOver > 0)
torsoTarget = rightHandOTarget.position + dShoulderNeck;
if (rightOver > 0 || leftOver > 0) {
bendAngle = BendAngle(torsoTarget, rightArm); // arm should be left or right
spine.rotation = spineStartRotation * Quaternion.AngleAxis(bendAngle, spineAxis);
} else
spine.rotation = spineStartRotation;
rightArm.Calculate(rightHandTarget);
leftArm.Calculate(leftHandTarget);
return bendAngle;
}
public Torso(Transform neck, Transform spine, Transform hips, Transform hipTarget, Transform neckTarget, ArmMovements arm) {
this.hipTarget = hipTarget;
this.neck = neck;
neckStartPosition = neck.position;
this.spine = spine;
spineStartRotation = spine.rotation;
spineStartOrientation = neck.position - spine.position;
this.hips = hips;
hipStartPosition = hips.position;
Vector3 neckAtUpperArm = new Vector3(neck.position.x, arm.upperArm.position.y, neck.position.z);
length = Vector3.Distance(spine.position, neckAtUpperArm);
fromNormNeck = Quaternion.Inverse(Quaternion.LookRotation(hipTarget.forward)) * neck.rotation;
fromNormTorso = Quaternion.Inverse(Quaternion.LookRotation(neck.position - spine.position, hipTarget.forward)) * spine.rotation;
fromNormHips = Quaternion.Inverse(Quaternion.LookRotation(hipTarget.forward)) * hips.rotation;
userNeckTarget = (neckTarget != null);
spineAxis = spine.InverseTransformDirection(hipTarget.right);
}
public void CalculateFromNormTracking(ArmMovements leftArm, ArmMovements rightArm, Transform leftHandTarget, Transform rightHandTarget) {
if (leftArm != null) {
leftArm.CalculateFromNormTracking(leftHandTarget);
}
if (rightArm != null) {
rightArm.CalculateFromNormTracking(rightHandTarget);
}
fromNormCalculated = true;
}
public void CalculateFromNormTPose(ArmMovements leftArm, ArmMovements rightArm) {
if (leftArm != null) {
leftArm.CalculateFromNormTPose();
}
if (rightArm != null) {
rightArm.CalculateFromNormTPose();
}
fromNormCalculated = true;
}
public bool IsInTPose(ArmMovements leftArm, ArmMovements rightArm) {
float d;
Ray hand2hand = new Ray(leftArm.hand.position, rightArm.hand.position - leftArm.hand.position);
// All lined up?
d = DistanceToRay(hand2hand, leftArm.forearm.position);
if (d > 0.1f) return false;
d = DistanceToRay(hand2hand, leftArm.upperArm.position);
if (d > 0.1f) return false;
d = DistanceToRay(hand2hand, rightArm.upperArm.position);
if (d > 0.1f) return false;
d = DistanceToRay(hand2hand, rightArm.forearm.position);
if (d > 0.1f) return false;
// Arms stretched?
d = Vector3.Distance(leftArm.upperArm.position, leftArm.hand.position);
if (d < leftArm.length - 0.1f) return false;
d = Vector3.Distance(rightArm.upperArm.position, rightArm.hand.position);
if (d < rightArm.length - 0.1f) return false;
Debug.Log("Avatar is in T-pose");
return true;
}
