public static Rotation CalculateUpperArmOrientation(Vector upperArmPosition, float upperArmLength, Vector forearmUp, float forearmLength, Vector handPosition, bool isLeft)
{
Rotation upperArmRotation = Rotation.LookRotation(handPosition - upperArmPosition, forearmUp);
float upperArm2HandDistance = Vector.Distance(upperArmPosition, handPosition);
float upperArm2HandDistance2 = upperArm2HandDistance * upperArm2HandDistance;
float upperArmLength2 = upperArmLength * upperArmLength;
float forearmLength2 = forearmLength * forearmLength;
float elbowAngle = (float)Math.Acos((upperArm2HandDistance2 + upperArmLength2 - forearmLength2) / (2 * upperArm2HandDistance * upperArmLength)) * Rad2Deg;
if (float.IsNaN(elbowAngle))
{
elbowAngle = 10;
}
if (isLeft)
{
elbowAngle = -elbowAngle;
}
upperArmRotation = Rotation.AngleAxis(elbowAngle, upperArmRotation * Vector.up) * upperArmRotation;
if (isLeft)
{
upperArmRotation *= Rotation.AngleAxis(90, Vector.up); // Euler(0, 90, 0)
}
else
{
upperArmRotation *= Rotation.AngleAxis(-90, Vector.up); // Euler(0, -90, 0)
}
return(upperArmRotation);
}
public static Rotation CalculateBoneRotation(Vector bonePosition, Vector parentBonePosition, Vector upDirection)
{
Vector direction = bonePosition - parentBonePosition;
if (Vector.Magnitude(direction) > 0)
{
return(Rotation.LookRotation(direction, upDirection));
}
else
{
return(Rotation.identity);
}
}
public static Rotation CalculateArmOrientation(Vector joint1Position, Vector joint1Up, Vector joint2Position, bool isLeft)
{
Vector boneForward = joint2Position - joint1Position;
Rotation boneRotation = Rotation.LookRotation(boneForward, joint1Up);
if (isLeft)
{
boneRotation *= Rotation.AngleAxis(90, Vector.up); // Euler(0, 90, 0)
}
else
{
boneRotation *= Rotation.AngleAxis(-90, Vector.up); // Euler(0, -90, 0)
}
return(boneRotation);
}
public static Rotation GetHeadOrientation()
{
if (faceIsTracked)
{
return(GetFaceOrientation());
}
else
{
Vector neckPos = GetTargetPosition(JointID.Neck);
Vector headPos = GetTargetPosition(JointID.Head);
Vector direction = headPos - neckPos;
direction = Rotation.AngleAxis(180, Vector.up) * direction;
Rotation neckRotation = Rotation.LookRotation(direction, Vector.forward);
neckRotation *= Rotation.AngleAxis(90, Vector.right);
return(neckRotation);
}
}
