private Vector3 GuessPalmToThumbAxis(Transform hand, Transform forearm)
{
if (hand.childCount == 0)
{
Debug.LogWarning("Hand " + hand.name + " does not have any fingers, VRIK can not guess the hand bone's orientation. Please assign 'Wrist To Palm Axis' and 'Palm To Thumb Axis' manually for both arms in VRIK settings.", hand);
return(Vector3.zero);
}
float closestSqrMag = Mathf.Infinity;
int thumbIndex = 0;
for (int i = 0; i < hand.childCount; i++)
{
float sqrMag = Vector3.SqrMagnitude(hand.GetChild(i).position - hand.position);
if (sqrMag < closestSqrMag)
{
closestSqrMag = sqrMag;
thumbIndex = i;
}
}
Vector3 handNormal = Vector3.Cross(hand.position - forearm.position, hand.GetChild(thumbIndex).position - hand.position);
Vector3 toThumb = Vector3.Cross(handNormal, hand.position - forearm.position);
Vector3 axis = AxisTools.ToVector3(AxisTools.GetAxisToDirection(hand, toThumb));
if (Vector3.Dot(toThumb, hand.rotation * axis) < 0f)
{
axis = -axis;
}
return(axis);
}
private static void CreateHandCollider(Transform hand, Transform lowerArm, Transform root, Options options)
{
Vector3 axis = hand.TransformVector(AxisTools.GetAxisVectorToPoint(hand, GetChildCentroid(hand, lowerArm.position)));
Vector3 endPoint = hand.position - (lowerArm.position - hand.position) * 0.75f;
endPoint = hand.position + Vector3.Project(endPoint - hand.position, axis).normalized *(endPoint - hand.position).magnitude;
CreateCollider(hand, hand.position, endPoint, options.handColliders, options.colliderLengthOverlap, Vector3.Distance(endPoint, hand.position) * 0.5f);
}
private Vector3 GuessWristToPalmAxis(Transform hand, Transform forearm)
{
Vector3 toForearm = forearm.position - hand.position;
Vector3 axis = AxisTools.ToVector3(AxisTools.GetAxisToDirection(hand, toForearm));
if (Vector3.Dot(toForearm, hand.rotation * axis) > 0f)
{
axis = -axis;
}
return(axis);
}
public static JointAxis GetSymmetricJointAxis(Transform jointTransform, Vector3 axis, Vector3 secondaryAxis, JointAxis jointAxis, Transform symmetricTransform, Vector3 symmetricAxis, Vector3 symmetricSecondaryAxis, Transform root)
{
Vector3 a = JointAxisToVector3(axis, secondaryAxis, jointAxis);
Vector3 aWorld = jointTransform.rotation * a;
Vector3 aWorldMirror = SymmetryTools.Mirror(aWorld, root);
Vector3 aLocalMirror = Quaternion.Inverse(symmetricTransform.rotation) * aWorldMirror;
Vector3 sAVector = AxisTools.GetAxisVectorToDirection(symmetricTransform, aWorldMirror);
Vector3 sA = Vector3.Project(aLocalMirror, sAVector);
return(GetJointAxis(symmetricAxis, symmetricSecondaryAxis, sA));
}
private static void CalibrateLeg(Settings settings, Transform FootTransform, IKSolverVR.Leg leg, Transform lastBone, Vector3 rootForward, bool isLeft)
{
Transform footAdjusterTransform = leg.target == null ? new GameObject(isLeft ? "leftFootAdjuster" : "rightFootAdjuster").transform : leg.target;
footAdjusterTransform.parent = FootTransform;
// Space of the tracker heading
Quaternion frontQuaternion = FootTransform.rotation * Quaternion.LookRotation(settings.footTrackerForward, settings.footTrackerUp);
Vector3 frontVector = frontQuaternion * Vector3.forward;
frontVector.y = 0f;
frontQuaternion = Quaternion.LookRotation(frontVector);
// Target position
float inwardOffset = isLeft ? settings.footInwardOffset : -settings.footInwardOffset;
footAdjusterTransform.position = FootTransform.position + frontQuaternion * new Vector3(inwardOffset, 0f, settings.footForwardOffset);
footAdjusterTransform.position = new Vector3(footAdjusterTransform.position.x, lastBone.position.y, footAdjusterTransform.position.z);
// Target rotation
footAdjusterTransform.rotation = lastBone.rotation;
// Rotate target forward towards tracker forward
Vector3 footForward = AxisTools.GetAxisVectorToDirection(lastBone, rootForward);
if (Vector3.Dot(lastBone.rotation * footForward, rootForward) < 0f)
{
footForward = -footForward;
}
Vector3 fLocal = Quaternion.Inverse(Quaternion.LookRotation(footAdjusterTransform.rotation * footForward)) * frontVector;
float angle = Mathf.Atan2(fLocal.x, fLocal.z) * Mathf.Rad2Deg;
float headingOffset = isLeft ? settings.footHeadingOffset : -settings.footHeadingOffset;
footAdjusterTransform.rotation = Quaternion.AngleAxis(angle + headingOffset, Vector3.up) * footAdjusterTransform.rotation;
leg.target = footAdjusterTransform;
leg.positionWeight = 1f;
leg.rotationWeight = 1f;
// Bend goal
/*
* Transform bendGoal = leg.bendGoal == null ? (new GameObject(name + " Leg Bend Goal")).transform : leg.bendGoal;
* bendGoal.position = lastBone.position + frontQuaternion * Vector3.forward + frontQuaternion * Vector3.up;// * 0.5f;
* bendGoal.parent = FootTransform;
* leg.bendGoal = bendGoal;
* leg.bendGoalWeight = 1f;
*/
leg.bendGoal = null;
leg.bendGoalWeight = 0f;
}
public static Vector3 DeltaSize(Vector3 size, Transform t, Vector3 direction, float delta)
{
Axis axis = AxisTools.GetAxisToDirection(t, direction);
switch (axis)
{
case Axis.X: size.x += delta; break;
case Axis.Y: size.y += delta; break;
case Axis.Z: size.z += delta; break;
}
return(size);
}
private static void CalibrateLeg(Settings settings, Transform tracker, IKSolverVR.Leg leg, Transform lastBone, Vector3 rootForward, bool isLeft)
{
string name = isLeft ? "Left" : "Right";
Transform target = leg.target == null ? (new GameObject(name + " Foot Target")).transform : leg.target;
// Space of the tracker heading
Quaternion trackerSpace = tracker.rotation * Quaternion.LookRotation(settings.footTrackerForward, settings.footTrackerUp);
Vector3 f = trackerSpace * Vector3.forward;
f.y = 0f;
trackerSpace = Quaternion.LookRotation(f);
// Target position
float inwardOffset = isLeft ? settings.footInwardOffset : -settings.footInwardOffset;
target.position = tracker.position + trackerSpace * new Vector3(inwardOffset, 0f, settings.footForwardOffset);
target.position = new Vector3(target.position.x, lastBone.position.y, target.position.z);
// Target rotation
target.rotation = lastBone.rotation;
// Rotate target forward towards tracker forward
Vector3 footForward = AxisTools.GetAxisVectorToDirection(lastBone, rootForward);
if (Vector3.Dot(lastBone.rotation * footForward, rootForward) < 0f)
{
footForward = -footForward;
}
Vector3 fLocal = Quaternion.Inverse(Quaternion.LookRotation(target.rotation * footForward)) * f;
float angle = Mathf.Atan2(fLocal.x, fLocal.z) * Mathf.Rad2Deg;
float headingOffset = isLeft ? settings.footHeadingOffset : -settings.footHeadingOffset;
target.rotation = Quaternion.AngleAxis(angle + headingOffset, Vector3.up) * target.rotation;
target.parent = tracker;
leg.target = target;
leg.positionWeight = 1f;
leg.rotationWeight = 1f;
// Bend goal
Transform bendGoal = leg.bendGoal == null ? (new GameObject(name + " Leg Bend Goal")).transform : leg.bendGoal;
bendGoal.position = lastBone.position + trackerSpace * Vector3.forward + trackerSpace * Vector3.up;// * 0.5f;
bendGoal.parent = tracker;
leg.bendGoal = bendGoal;
leg.bendGoalWeight = 1f;
}
protected static void CreateCollider(Transform t, Vector3 startPoint, Vector3 endPoint, ColliderType colliderType, float lengthOverlap, float width, float proportionAspect, Vector3 widthDirection)
{
if (colliderType == ColliderType.Capsule)
{
CreateCollider(t, startPoint, endPoint, colliderType, lengthOverlap, width * proportionAspect);
return;
}
Vector3 direction = endPoint - startPoint;
float height = direction.magnitude * (1f + lengthOverlap);
Vector3 heightAxis = AxisTools.GetAxisVectorToDirection(t, direction);
Vector3 widthAxis = AxisTools.GetAxisVectorToDirection(t, widthDirection);
if (widthAxis == heightAxis)
{
Debug.LogWarning("Width axis = height axis on " + t.name, t);
widthAxis = new Vector3(heightAxis.y, heightAxis.z, heightAxis.x);
}
t.gameObject.AddComponent <Rigidbody>();
Vector3 heightAdd = Vector3.Scale(heightAxis, new Vector3(height, height, height));
Vector3 widthAdd = Vector3.Scale(widthAxis, new Vector3(width, width, width));
Vector3 size = heightAdd + widthAdd;
if (size.x == 0f)
{
size.x = width * proportionAspect;
}
if (size.y == 0f)
{
size.y = width * proportionAspect;
}
if (size.z == 0f)
{
size.z = width * proportionAspect;
}
BoxCollider box = t.gameObject.AddComponent <BoxCollider>();
box.size = size / GetScaleF(t);
box.center = t.InverseTransformPoint(Vector3.Lerp(startPoint, endPoint, 0.5f));
}
protected static void CreateCollider(Transform t, Vector3 startPoint, Vector3 endPoint, ColliderType colliderType, float lengthOverlap, float width)
{
Vector3 direction = endPoint - startPoint;
float height = direction.magnitude * (1f + lengthOverlap);
Vector3 heightAxis = AxisTools.GetAxisVectorToDirection(t, direction);
t.gameObject.AddComponent <Rigidbody>();
float scaleF = GetScaleF(t);
switch (colliderType)
{
case ColliderType.Capsule:
CapsuleCollider capsule = t.gameObject.AddComponent <CapsuleCollider>();
capsule.height = Mathf.Abs(height / scaleF);
capsule.radius = Mathf.Abs((width * 0.75f) / scaleF);
capsule.direction = DirectionVector3ToInt(heightAxis);
capsule.center = t.InverseTransformPoint(Vector3.Lerp(startPoint, endPoint, 0.5f));
break;
case ColliderType.Box:
Vector3 size = Vector3.Scale(heightAxis, new Vector3(height, height, height));
if (size.x == 0f)
{
size.x = width;
}
if (size.y == 0f)
{
size.y = width;
}
if (size.z == 0f)
{
size.z = width;
}
BoxCollider box = t.gameObject.AddComponent <BoxCollider>();
box.size = size / scaleF;
box.size = new Vector3(Mathf.Abs(box.size.x), Mathf.Abs(box.size.y), Mathf.Abs(box.size.z));
box.center = t.InverseTransformPoint(Vector3.Lerp(startPoint, endPoint, 0.5f));
break;
}
}
private static void CreateFootCollider(Transform foot, Transform lowerLeg, Transform upperLeg, Transform root, Options options)
{
float legHeight = (upperLeg.position - foot.position).magnitude;
Vector3 axis = foot.TransformVector(AxisTools.GetAxisVectorToPoint(foot, GetChildCentroid(foot, foot.position + root.forward) + root.forward * legHeight * 0.2f));
Vector3 endPoint = foot.position + root.forward * legHeight * 0.25f;
endPoint = foot.position + Vector3.Project(endPoint - foot.position, axis).normalized *(endPoint - foot.position).magnitude;
float width = Vector3.Distance(endPoint, foot.position) * 0.5f;
Vector3 startPoint = foot.position;
bool footBelowRoot = Vector3.Dot(root.up, foot.position - root.position) < 0f;
Vector3 heightOffset = footBelowRoot? Vector3.zero: Vector3.Project((startPoint - root.up * width * 0.5f) - root.position, root.up);
Vector3 direction = endPoint - startPoint;
startPoint -= direction * 0.2f;
CreateCollider(foot, startPoint - heightOffset, endPoint - heightOffset, options.footColliders, options.colliderLengthOverlap, width);
}
private static Axis GetViewAxis(Joint joint)
{
if (joint.connectedBody == null)
{
return(Axis.Z);
}
CapsuleCollider capsule = joint.GetComponent <CapsuleCollider>();
if (capsule != null && capsule.center != Vector3.zero)
{
return(AxisTools.ToAxis(capsule.center));
}
else
{
BoxCollider box = joint.GetComponent <BoxCollider>();
if (box != null && box.center != Vector3.zero)
{
return(AxisTools.ToAxis(box.center));
}
}
return(AxisTools.GetAxisToPoint(joint.transform, joint.connectedBody.worldCenterOfMass));
}
private static void CreateColliders(BipedRagdollReferences r, Options options)
{
// Torso
Vector3 upperArmToHeadCentroid = GetUpperArmToHeadCentroid(r);
if (r.spine == null)
{
options.spine = false;
}
if (r.chest == null)
{
options.chest = false;
}
Vector3 shoulderDirection = r.rightUpperArm.position - r.leftUpperArm.position;
float torsoWidth = shoulderDirection.magnitude;
float torsoProportionAspect = 0.6f;
// Hips
Vector3 hipsStartPoint = r.hips.position;
// Making sure the hip bone is not at the feet
float toHead = Vector3.Distance(r.head.position, r.root.position);
float toHips = Vector3.Distance(r.hips.position, r.root.position);
if (toHips < toHead * 0.2f)
{
hipsStartPoint = Vector3.Lerp(r.leftUpperLeg.position, r.rightUpperLeg.position, 0.5f);
}
Vector3 lastEndPoint = options.spine? r.spine.position: (options.chest? r.chest.position: upperArmToHeadCentroid);
hipsStartPoint += (hipsStartPoint - upperArmToHeadCentroid) * 0.1f;
float hipsWidth = options.spine || options.chest? torsoWidth * 0.8f: torsoWidth;
CreateCollider(r.hips, hipsStartPoint, lastEndPoint, options.torsoColliders, options.colliderLengthOverlap, hipsWidth, torsoProportionAspect, shoulderDirection);
// Spine
if (options.spine)
{
Vector3 spineStartPoint = lastEndPoint;
lastEndPoint = options.chest? r.chest.position: upperArmToHeadCentroid;
float spineWidth = options.chest? torsoWidth * 0.75f: torsoWidth;
CreateCollider(r.spine, spineStartPoint, lastEndPoint, options.torsoColliders, options.colliderLengthOverlap, spineWidth, torsoProportionAspect, shoulderDirection);
}
if (options.chest)
{
Vector3 chestStartPoint = lastEndPoint;
lastEndPoint = upperArmToHeadCentroid;
CreateCollider(r.chest, chestStartPoint, lastEndPoint, options.torsoColliders, options.colliderLengthOverlap, torsoWidth, torsoProportionAspect, shoulderDirection);
}
// Head
Vector3 headStartPoint = lastEndPoint;
Vector3 headEndPoint = headStartPoint + (headStartPoint - hipsStartPoint) * 0.45f;
Vector3 axis = r.head.TransformVector(AxisTools.GetAxisVectorToDirection(r.head, headEndPoint - headStartPoint));
headEndPoint = headStartPoint + Vector3.Project(headEndPoint - headStartPoint, axis).normalized *(headEndPoint - headStartPoint).magnitude;
CreateCollider(r.head, headStartPoint, headEndPoint, options.headCollider, options.colliderLengthOverlap, Vector3.Distance(headStartPoint, headEndPoint) * 0.8f);
// Arms
float armWidthAspect = 0.4f;
float leftArmWidth = Vector3.Distance(r.leftUpperArm.position, r.leftLowerArm.position) * armWidthAspect;
CreateCollider(r.leftUpperArm, r.leftUpperArm.position, r.leftLowerArm.position, options.armColliders, options.colliderLengthOverlap, leftArmWidth);
CreateCollider(r.leftLowerArm, r.leftLowerArm.position, r.leftHand.position, options.armColliders, options.colliderLengthOverlap, leftArmWidth * 0.9f);
float rightArmWidth = Vector3.Distance(r.rightUpperArm.position, r.rightLowerArm.position) * armWidthAspect;
CreateCollider(r.rightUpperArm, r.rightUpperArm.position, r.rightLowerArm.position, options.armColliders, options.colliderLengthOverlap, rightArmWidth);
CreateCollider(r.rightLowerArm, r.rightLowerArm.position, r.rightHand.position, options.armColliders, options.colliderLengthOverlap, rightArmWidth * 0.9f);
// Legs
float legWidthAspect = 0.3f;
float leftLegWidth = Vector3.Distance(r.leftUpperLeg.position, r.leftLowerLeg.position) * legWidthAspect;
CreateCollider(r.leftUpperLeg, r.leftUpperLeg.position, r.leftLowerLeg.position, options.legColliders, options.colliderLengthOverlap, leftLegWidth);
CreateCollider(r.leftLowerLeg, r.leftLowerLeg.position, r.leftFoot.position, options.legColliders, options.colliderLengthOverlap, leftLegWidth * 0.9f);
float rightLegWidth = Vector3.Distance(r.rightUpperLeg.position, r.rightLowerLeg.position) * legWidthAspect;
CreateCollider(r.rightUpperLeg, r.rightUpperLeg.position, r.rightLowerLeg.position, options.legColliders, options.colliderLengthOverlap, rightLegWidth);
CreateCollider(r.rightLowerLeg, r.rightLowerLeg.position, r.rightFoot.position, options.legColliders, options.colliderLengthOverlap, rightLegWidth * 0.9f);
// Hands
if (options.hands)
{
CreateHandCollider(r.leftHand, r.leftLowerArm, r.root, options);
CreateHandCollider(r.rightHand, r.rightLowerArm, r.root, options);
}
// Feet
if (options.feet)
{
CreateFootCollider(r.leftFoot, r.leftLowerLeg, r.leftUpperLeg, r.root, options);
CreateFootCollider(r.rightFoot, r.rightLowerLeg, r.rightUpperLeg, r.root, options);
}
}
protected override void OnRead(Vector3[] positions, Quaternion[] rotations, bool hasChest, bool hasNeck, bool hasShoulders, bool hasToes, bool hasLegs, int rootIndex, int index)
{
Vector3 shoulderPosition = positions[index];
Quaternion shoulderRotation = rotations[index];
Vector3 upperArmPosition = positions[index + 1];
Quaternion upperArmRotation = rotations[index + 1];
Vector3 forearmPosition = positions[index + 2];
Quaternion forearmRotation = rotations[index + 2];
Vector3 handPosition = positions[index + 3];
Quaternion handRotation = rotations[index + 3];
if (!initiated)
{
IKPosition = handPosition;
IKRotation = handRotation;
rotation = IKRotation;
this.hasShoulder = hasShoulders;
bones = new VirtualBone[hasShoulder ? 4 : 3];
if (hasShoulder)
{
bones[0] = new VirtualBone(shoulderPosition, shoulderRotation);
bones[1] = new VirtualBone(upperArmPosition, upperArmRotation);
bones[2] = new VirtualBone(forearmPosition, forearmRotation);
bones[3] = new VirtualBone(handPosition, handRotation);
}
else
{
this.bones[0] = new VirtualBone(upperArmPosition, upperArmRotation);
this.bones[1] = new VirtualBone(forearmPosition, forearmRotation);
this.bones[2] = new VirtualBone(handPosition, handRotation);
}
Vector3 rootForward = rotations[0] * Vector3.forward;
chestForwardAxis = Quaternion.Inverse(rootRotation) * rootForward;
chestUpAxis = Quaternion.Inverse(rootRotation) * (rotations[0] * Vector3.up);
// Get the local axis of the upper arm pointing towards the bend normal
Vector3 upperArmForwardAxis = AxisTools.GetAxisVectorToDirection(upperArmRotation, rootForward);
if (Vector3.Dot(upperArmRotation * upperArmForwardAxis, rootForward) < 0f)
{
upperArmForwardAxis = -upperArmForwardAxis;
}
upperArmBendAxis = Vector3.Cross(Quaternion.Inverse(upperArmRotation) * (forearmPosition - upperArmPosition), upperArmForwardAxis);
if (upperArmBendAxis == Vector3.zero)
{
Debug.LogWarning("VRIK can not calculate which way to bend the arms because the arms are perfectly straight. Please rotate the elbow bones slightly in their natural bending direction in the Editor.");
}
}
if (hasShoulder)
{
bones[0].Read(shoulderPosition, shoulderRotation);
bones[1].Read(upperArmPosition, upperArmRotation);
bones[2].Read(forearmPosition, forearmRotation);
bones[3].Read(handPosition, handRotation);
}
else
{
bones[0].Read(upperArmPosition, upperArmRotation);
bones[1].Read(forearmPosition, forearmRotation);
bones[2].Read(handPosition, handRotation);
}
}
protected override void OnRead(Vector3[] positions, Quaternion[] rotations, bool hasChest, bool hasNeck, bool hasShoulders, bool hasToes, bool hasLegs, int rootIndex, int index)
{
Vector3 shoulderPosition = positions[index];
Quaternion shoulderRotation = rotations[index];
Vector3 upperArmPosition = positions[index + 1];
Quaternion upperArmRotation = rotations[index + 1];
Vector3 forearmPosition = positions[index + 2];
Quaternion forearmRotation = rotations[index + 2];
Vector3 handPosition = positions[index + 3];
Quaternion handRotation = rotations[index + 3];
if (!initiated)
{
IKPosition = handPosition;
IKRotation = handRotation;
rotation = IKRotation;
this.hasShoulder = hasShoulders;
bones = new VirtualBone[hasShoulder ? 4 : 3];
if (hasShoulder)
{
bones[0] = new VirtualBone(shoulderPosition, shoulderRotation);
bones[1] = new VirtualBone(upperArmPosition, upperArmRotation);
bones[2] = new VirtualBone(forearmPosition, forearmRotation);
bones[3] = new VirtualBone(handPosition, handRotation);
}
else
{
this.bones[0] = new VirtualBone(upperArmPosition, upperArmRotation);
this.bones[1] = new VirtualBone(forearmPosition, forearmRotation);
this.bones[2] = new VirtualBone(handPosition, handRotation);
}
Vector3 rootForward = rotations[0] * Vector3.forward;
chestForwardAxis = Quaternion.Inverse(rootRotation) * rootForward;
chestUpAxis = Quaternion.Inverse(rootRotation) * (rotations[0] * Vector3.up);
// Get the local axis of the upper arm pointing towards the bend normal
Vector3 upperArmForwardAxis = AxisTools.GetAxisVectorToDirection(upperArmRotation, rootForward);
if (Vector3.Dot(upperArmRotation * upperArmForwardAxis, rootForward) < 0f)
{
upperArmForwardAxis = -upperArmForwardAxis;
}
upperArmBendAxis = Vector3.Cross(Quaternion.Inverse(upperArmRotation) * (forearmPosition - upperArmPosition), upperArmForwardAxis);
}
if (hasShoulder)
{
bones[0].Read(shoulderPosition, shoulderRotation);
bones[1].Read(upperArmPosition, upperArmRotation);
bones[2].Read(forearmPosition, forearmRotation);
bones[3].Read(handPosition, handRotation);
}
else
{
bones[0].Read(upperArmPosition, upperArmRotation);
bones[1].Read(forearmPosition, forearmRotation);
bones[2].Read(handPosition, handRotation);
}
}