/*
* Gets the starting position of the iteration
* */
private Vector3 GetPosition(IKSolverFullBody solver, out Quaternion planeRotationOffset)
{
planeRotationOffset = Quaternion.identity;
if (!isEndEffector)
{
return(solver.GetNode(chainIndex, nodeIndex).solverPosition); // non end-effectors are always free
}
if (maintainRelativePositionWeight <= 0f)
{
return(animatedPosition);
}
// Maintain relative position
Vector3 p = bone.position;
Vector3 dir = p - planeBone1.position;
planeRotationOffset = GetPlaneRotation(solver) * Quaternion.Inverse(animatedPlaneRotation);
p = solver.GetNode(plane1ChainIndex, plane1NodeIndex).solverPosition + planeRotationOffset * dir;
// Interpolate the rotation offset
planeRotationOffset = Quaternion.Lerp(Quaternion.identity, planeRotationOffset, maintainRelativePositionWeight);
return(Vector3.Lerp(animatedPosition, p + solver.GetNode(chainIndex, nodeIndex).offset, maintainRelativePositionWeight));
}
/*
* Positioning and rotating the spine bones to match the solver positions
* */
private void MapToSolverPositions(IKSolverFullBody solver)
{
// Translating the first bone
// Note: spine here also includes the pelvis
spine[0].SetToIKPosition();
spine[0].RotateToPlane(solver, 1f);
// Translating all the bones between the first and the last
for (int i = 1; i < spine.Length - 1; i++)
{
spine[i].Swing(spine[i + 1].ikPosition, 1f);
if (twistWeight > 0)
{
float bWeight = (float)i / ((float)spine.Length - 2);
Vector3 s1 = solver.GetNode(leftUpperArm.chainIndex, leftUpperArm.nodeIndex).solverPosition;
Vector3 s2 = solver.GetNode(rightUpperArm.chainIndex, rightUpperArm.nodeIndex).solverPosition;
spine[i].Twist(s1 - s2, spine[i + 1].ikPosition - spine[i].transform.position, bWeight * twistWeight);
}
}
// Translating the last bone
spine[spine.Length - 1].SetToIKPosition();
spine[spine.Length - 1].RotateToPlane(solver, 1f);
}
/*
* Clear node offset
* */
public void ResetOffset(IKSolverFullBody solver) {
solver.GetNode(chainIndex, nodeIndex).offset = Vector3.zero;
for (int i = 0; i < childChainIndexes.Length; i++) {
solver.GetNode(childChainIndexes[i], childNodeIndexes[i]).offset = Vector3.zero;
}
}
private Quaternion GetTargetRotation(IKSolverFullBody solver)
{
Vector3 solverPosition = solver.GetNode(this.plane1ChainIndex, this.plane1NodeIndex).solverPosition;
Vector3 solverPosition2 = solver.GetNode(this.plane2ChainIndex, this.plane2NodeIndex).solverPosition;
Vector3 solverPosition3 = solver.GetNode(this.plane3ChainIndex, this.plane3NodeIndex).solverPosition;
if (solverPosition == solverPosition3)
{
return(Quaternion.identity);
}
return(Quaternion.LookRotation(solverPosition2 - solverPosition, solverPosition3 - solverPosition));
}
/*
* Rotation of plane nodes in the solver
* */
private Quaternion GetPlaneRotation(IKSolverFullBody solver) {
Vector3 p1 = solver.GetNode(plane1ChainIndex, plane1NodeIndex).solverPosition;
Vector3 p2 = solver.GetNode(plane2ChainIndex, plane2NodeIndex).solverPosition;
Vector3 p3 = solver.GetNode(plane3ChainIndex, plane3NodeIndex).solverPosition;
Vector3 viewingVector = p2 - p1;
Vector3 upVector = p3 - p1;
if (viewingVector == Vector3.zero) {
Warning.Log("Make sure you are not placing 2 or more FBBIK effectors of the same chain to exactly the same position.", bone);
return Quaternion.identity;
}
return Quaternion.LookRotation(viewingVector, upVector);
}
private Quaternion GetPlaneRotation(IKSolverFullBody solver)
{
Vector3 solverPosition = solver.GetNode(this.plane1ChainIndex, this.plane1NodeIndex).solverPosition;
Vector3 solverPosition2 = solver.GetNode(this.plane2ChainIndex, this.plane2NodeIndex).solverPosition;
Vector3 solverPosition3 = solver.GetNode(this.plane3ChainIndex, this.plane3NodeIndex).solverPosition;
Vector3 vector = solverPosition2 - solverPosition;
Vector3 upwards = solverPosition3 - solverPosition;
if (vector == Vector3.zero)
{
Warning.Log("Make sure you are not placing 2 or more FBBIK effectors of the same chain to exactly the same position.", this.bone, false);
return(Quaternion.identity);
}
return(Quaternion.LookRotation(vector, upwards));
}
/*
* Manipulating node solverPosition
* */
public void Update(IKSolverFullBody solver) {
if (firstUpdate) {
animatedPosition = bone.position + solver.GetNode(chainIndex, nodeIndex).offset;
firstUpdate = false;
}
solver.GetNode(chainIndex, nodeIndex).solverPosition = Vector3.Lerp(GetPosition(solver, out planeRotationOffset), position, posW);
// Child nodes
if (!effectChildNodes) return;
for (int i = 0; i < childBones.Length; i++) {
solver.GetNode(childChainIndexes[i], childNodeIndexes[i]).solverPosition = Vector3.Lerp(solver.GetNode(childChainIndexes[i], childNodeIndexes[i]).solverPosition, solver.GetNode(chainIndex, nodeIndex).solverPosition + localPositions[i], posW);
}
}
/*
* Rotation of plane nodes in the solver
* */
private Quaternion GetTargetRotation(IKSolverFullBody solver)
{
Vector3 p1 = solver.GetNode(plane1ChainIndex, plane1NodeIndex).solverPosition;
Vector3 p2 = solver.GetNode(plane2ChainIndex, plane2NodeIndex).solverPosition;
Vector3 p3 = solver.GetNode(plane3ChainIndex, plane3NodeIndex).solverPosition;
if (p1 == p3)
{
return(Quaternion.identity);
}
return(Quaternion.LookRotation(p2 - p1, p3 - p1));
//if (planeNode1.solverPosition == planeNode3.solverPosition) return Quaternion.identity;
//return Quaternion.LookRotation(planeNode2.solverPosition - planeNode1.solverPosition, planeNode3.solverPosition - planeNode1.solverPosition);
}
/*
* Presolving, applying offset
* */
public void OnPreSolve(IKSolverFullBody solver) {
positionWeight = Mathf.Clamp(positionWeight, 0f, 1f);
rotationWeight = Mathf.Clamp(rotationWeight, 0f, 1f);
maintainRelativePositionWeight = Mathf.Clamp(maintainRelativePositionWeight, 0f, 1f);
// Calculating weights
posW = positionWeight * solver.IKPositionWeight;
rotW = rotationWeight * solver.IKPositionWeight;
solver.GetNode(chainIndex, nodeIndex).effectorPositionWeight = posW;
solver.GetNode(chainIndex, nodeIndex).effectorRotationWeight = rotW;
solver.GetNode(chainIndex, nodeIndex).solverRotation = rotation;
if (float.IsInfinity(positionOffset.x) ||
float.IsInfinity(positionOffset.y) ||
float.IsInfinity(positionOffset.z)
) Debug.LogError("Invalid IKEffector.positionOffset (contains Infinity)! Please make sure not to set IKEffector.positionOffset to infinite values.", bone);
if (float.IsNaN(positionOffset.x) ||
float.IsNaN(positionOffset.y) ||
float.IsNaN(positionOffset.z)
) Debug.LogError("Invalid IKEffector.positionOffset (contains NaN)! Please make sure not to set IKEffector.positionOffset to NaN values.", bone);
if (positionOffset.sqrMagnitude > 10000000000f) Debug.LogError("Additive effector positionOffset detected in Full Body IK (extremely large value). Make sure you are not circularily adding to effector positionOffset each frame.", bone);
if (float.IsInfinity(position.x) ||
float.IsInfinity(position.y) ||
float.IsInfinity(position.z)
) Debug.LogError("Invalid IKEffector.position (contains Infinity)!");
solver.GetNode(chainIndex, nodeIndex).offset += positionOffset * solver.IKPositionWeight;
if (effectChildNodes && solver.iterations > 0) {
for (int i = 0; i < childBones.Length; i++) {
localPositions[i] = childBones[i].transform.position - bone.transform.position;
solver.GetNode(childChainIndexes[i], childNodeIndexes[i]).offset += positionOffset * solver.IKPositionWeight;
}
}
// Relative to Plane
if (usePlaneNodes && maintainRelativePositionWeight > 0f) {
animatedPlaneRotation = Quaternion.LookRotation(planeBone2.position - planeBone1.position, planeBone3.position - planeBone1.position);;
}
firstUpdate = true;
}
private Vector3 OrthoToBone1(IKSolverFullBody solver, Vector3 tangent)
{
Vector3 vector = solver.GetNode(this.chainIndex2, this.nodeIndex2).solverPosition - solver.GetNode(this.chainIndex1, this.nodeIndex1).solverPosition;
Vector3.OrthoNormalize(ref vector, ref tangent);
return(tangent);
}
public void WritePose(IKSolverFullBody solver, bool fullBody)
{
if (weight <= 0f)
{
return;
}
// Swing the parent bone to look at the first node's position
if (fullBody)
{
if (parentBone != null)
{
boneMapParent.Swing(solver.GetNode(boneMap1.chainIndex, boneMap1.nodeIndex).solverPosition, weight);
//boneMapParent.Swing(boneMap1.node.solverPosition, weight);
}
// Fix the first bone to it's node
boneMap1.FixToNode(solver, weight);
}
// Rotate the 2 first bones to the plane points
boneMap1.RotateToPlane(solver, weight);
boneMap2.RotateToPlane(solver, weight);
// Rotate the third bone to the rotation it had before solving
boneMap3.RotateToMaintain(maintainRotationWeight * weight * solver.IKPositionWeight);
// Rotate the third bone to the effector rotation
boneMap3.RotateToEffector(solver, weight);
}
public void Update(IKSolverFullBody solver)
{
if (this.firstUpdate)
{
this.animatedPosition = this.bone.position + solver.GetNode(this.chainIndex, this.nodeIndex).offset;
this.firstUpdate = false;
}
solver.GetNode(this.chainIndex, this.nodeIndex).solverPosition = Vector3.Lerp(this.GetPosition(solver, out this.planeRotationOffset), this.position, this.posW);
if (!this.effectChildNodes)
{
return;
}
for (int i = 0; i < this.childBones.Length; i++)
{
solver.GetNode(this.childChainIndexes[i], this.childNodeIndexes[i]).solverPosition = Vector3.Lerp(solver.GetNode(this.childChainIndexes[i], this.childNodeIndexes[i]).solverPosition, solver.GetNode(this.chainIndex, this.nodeIndex).solverPosition + this.localPositions[i], this.posW);
}
}
public void RotateToEffector(IKSolverFullBody solver, float weight)
{
if (!this.isNodeBone)
{
return;
}
float num = weight * solver.GetNode(this.chainIndex, this.nodeIndex).effectorRotationWeight;
if (num <= 0f)
{
return;
}
if (num >= 1f)
{
this.transform.rotation = solver.GetNode(this.chainIndex, this.nodeIndex).solverRotation;
return;
}
this.transform.rotation = Quaternion.Lerp(this.transform.rotation, solver.GetNode(this.chainIndex, this.nodeIndex).solverRotation, num);
}
private Vector3 GetPosition(IKSolverFullBody solver, out Quaternion planeRotationOffset)
{
planeRotationOffset = Quaternion.identity;
if (!this.isEndEffector)
{
return(solver.GetNode(this.chainIndex, this.nodeIndex).solverPosition);
}
if (this.maintainRelativePositionWeight <= 0f)
{
return(this.animatedPosition);
}
Vector3 a = this.bone.position;
Vector3 point = a - this.planeBone1.position;
planeRotationOffset = this.GetPlaneRotation(solver) * Quaternion.Inverse(this.animatedPlaneRotation);
a = solver.GetNode(this.plane1ChainIndex, this.plane1NodeIndex).solverPosition + planeRotationOffset * point;
planeRotationOffset = Quaternion.Lerp(Quaternion.identity, planeRotationOffset, this.maintainRelativePositionWeight);
return(Vector3.Lerp(this.animatedPosition, a + solver.GetNode(this.chainIndex, this.nodeIndex).offset, this.maintainRelativePositionWeight));
}
public void OnPreSolve(IKSolverFullBody solver)
{
this.positionWeight = Mathf.Clamp(this.positionWeight, 0f, 1f);
this.rotationWeight = Mathf.Clamp(this.rotationWeight, 0f, 1f);
this.maintainRelativePositionWeight = Mathf.Clamp(this.maintainRelativePositionWeight, 0f, 1f);
this.posW = this.positionWeight * solver.IKPositionWeight;
this.rotW = this.rotationWeight * solver.IKPositionWeight;
solver.GetNode(this.chainIndex, this.nodeIndex).effectorPositionWeight = this.posW;
solver.GetNode(this.chainIndex, this.nodeIndex).effectorRotationWeight = this.rotW;
solver.GetNode(this.chainIndex, this.nodeIndex).solverRotation = this.rotation;
if (float.IsInfinity(this.positionOffset.x) || float.IsInfinity(this.positionOffset.y) || float.IsInfinity(this.positionOffset.z))
{
Debug.LogError("Invalid IKEffector.positionOffset (contains Infinity)! Please make sure not to set IKEffector.positionOffset to infinite values.", this.bone);
}
if (float.IsNaN(this.positionOffset.x) || float.IsNaN(this.positionOffset.y) || float.IsNaN(this.positionOffset.z))
{
Debug.LogError("Invalid IKEffector.positionOffset (contains NaN)! Please make sure not to set IKEffector.positionOffset to NaN values.", this.bone);
}
if (this.positionOffset.sqrMagnitude > 1E+10f)
{
Debug.LogError("Additive effector positionOffset detected in Full Body IK (extremely large value). Make sure you are not circularily adding to effector positionOffset each frame.", this.bone);
}
if (float.IsInfinity(this.position.x) || float.IsInfinity(this.position.y) || float.IsInfinity(this.position.z))
{
Debug.LogError("Invalid IKEffector.position (contains Infinity)!");
}
solver.GetNode(this.chainIndex, this.nodeIndex).offset += this.positionOffset * solver.IKPositionWeight;
if (this.effectChildNodes && solver.iterations > 0)
{
for (int i = 0; i < this.childBones.Length; i++)
{
this.localPositions[i] = this.childBones[i].transform.position - this.bone.transform.position;
solver.GetNode(this.childChainIndexes[i], this.childNodeIndexes[i]).offset += this.positionOffset * solver.IKPositionWeight;
}
}
if (this.usePlaneNodes && this.maintainRelativePositionWeight > 0f)
{
this.animatedPlaneRotation = Quaternion.LookRotation(this.planeBone2.position - this.planeBone1.position, this.planeBone3.position - this.planeBone1.position);
}
this.firstUpdate = true;
}
/*
* Rotates to match the effector rotation
* */
public void RotateToEffector(IKSolverFullBody solver, float weight)
{
if (!isNodeBone)
{
return;
}
float w = weight * solver.GetNode(chainIndex, nodeIndex).effectorRotationWeight;
if (w <= 0f)
{
return;
}
if (w >= 1f)
{
transform.rotation = solver.GetNode(chainIndex, nodeIndex).solverRotation;
return;
}
transform.rotation = Quaternion.Lerp(transform.rotation, solver.GetNode(chainIndex, nodeIndex).solverRotation, w);
}
public Vector3 GetDir(IKSolverFullBody solver)
{
if (!this.initiated)
{
return(Vector3.zero);
}
float num = this.weight * solver.IKPositionWeight;
if (this.bendGoal != null)
{
Vector3 lhs = this.bendGoal.position - solver.GetNode(this.chainIndex1, this.nodeIndex1).solverPosition;
if (lhs != Vector3.zero)
{
this.direction = lhs;
}
}
if (num >= 1f)
{
return(this.direction.normalized);
}
Vector3 vector = solver.GetNode(this.chainIndex3, this.nodeIndex3).solverPosition - solver.GetNode(this.chainIndex1, this.nodeIndex1).solverPosition;
Quaternion rotation = Quaternion.FromToRotation(this.bone3.position - this.bone1.position, vector);
Vector3 vector2 = rotation * (this.bone2.position - this.bone1.position);
if (solver.GetNode(this.chainIndex3, this.nodeIndex3).effectorRotationWeight > 0f)
{
Vector3 b = -Vector3.Cross(vector, solver.GetNode(this.chainIndex3, this.nodeIndex3).solverRotation *this.defaultChildDirection);
vector2 = Vector3.Lerp(vector2, b, solver.GetNode(this.chainIndex3, this.nodeIndex3).effectorRotationWeight);
}
if (this.rotationOffset != Quaternion.identity)
{
Quaternion lhs2 = Quaternion.FromToRotation(this.rotationOffset * vector, vector);
vector2 = lhs2 * this.rotationOffset * vector2;
}
if (num <= 0f)
{
return(vector2);
}
return(Vector3.Lerp(vector2, this.direction.normalized, num));
}
public void FixToNode(IKSolverFullBody solver, float weight, IKSolver.Node fixNode = null)
{
if (fixNode == null)
{
fixNode = solver.GetNode(this.chainIndex, this.nodeIndex);
}
if (weight >= 1f)
{
this.transform.position = fixNode.solverPosition;
return;
}
this.transform.position = Vector3.Lerp(this.transform.position, fixNode.solverPosition, weight);
}
/*
* Mapping the spine to the hip and chest planes
* */
public void WritePose(IKSolverFullBody solver)
{
Vector3 firstPosition = spine[0].GetPlanePosition(solver);
Vector3 rootPosition = solver.GetNode(spine[rootNodeIndex].chainIndex, spine[rootNodeIndex].nodeIndex).solverPosition;
Vector3 lastPosition = spine[spine.Length - 1].GetPlanePosition(solver);
// If we have more than 3 bones, use the FABRIK algorithm
if (useFABRIK)
{
Vector3 offset = solver.GetNode(spine[rootNodeIndex].chainIndex, spine[rootNodeIndex].nodeIndex).solverPosition - spine[rootNodeIndex].transform.position;
for (int i = 0; i < spine.Length; i++)
{
spine[i].ikPosition = spine[i].transform.position + offset;
}
// Iterating the FABRIK algorithm
for (int i = 0; i < iterations; i++)
{
ForwardReach(lastPosition);
BackwardReach(firstPosition);
spine[rootNodeIndex].ikPosition = rootPosition;
}
}
else
{
// When we have just 3 bones, we know their positions already
spine[0].ikPosition = firstPosition;
spine[rootNodeIndex].ikPosition = rootPosition;
}
spine[spine.Length - 1].ikPosition = lastPosition;
// Mapping the spine bones to the solver
MapToSolverPositions(solver);
}
public void WritePose(IKSolverFullBody solver, bool fullBody)
{
if (this.weight <= 0f)
{
return;
}
if (fullBody)
{
if (this.parentBone != null)
{
this.boneMapParent.Swing(solver.GetNode(this.boneMap1.chainIndex, this.boneMap1.nodeIndex).solverPosition, this.weight);
}
this.boneMap1.FixToNode(solver, this.weight, null);
}
this.boneMap1.RotateToPlane(solver, this.weight);
this.boneMap2.RotateToPlane(solver, this.weight);
this.boneMap3.RotateToMaintain(this.maintainRotationWeight * this.weight * solver.IKPositionWeight);
this.boneMap3.RotateToEffector(solver, this.weight);
}
/*
* Rotates to match the effector rotation
* */
public void RotateToEffector(IKSolverFullBody solver, float weight) {
if (!isNodeBone) return;
float w = weight * solver.GetNode(chainIndex, nodeIndex).effectorRotationWeight;
if (w <= 0f) return;
if (w >= 1f) {
transform.rotation = solver.GetNode(chainIndex, nodeIndex).solverRotation;
return;
}
transform.rotation = Quaternion.Lerp(transform.rotation, solver.GetNode(chainIndex, nodeIndex).solverRotation, w);
}
/*
* Moves the bone to the solver position of it's node
* */
public void FixToNode(IKSolverFullBody solver, float weight, IKSolver.Node fixNode = null) {
if (fixNode == null) fixNode = solver.GetNode(chainIndex, nodeIndex);
if (weight >= 1f) {
transform.position = fixNode.solverPosition;
return;
}
transform.position = Vector3.Lerp(transform.position, fixNode.solverPosition, weight);
}
/*
* Mapping the spine to the hip and chest planes
* */
public void WritePose(IKSolverFullBody solver) {
Vector3 firstPosition = spine[0].GetPlanePosition(solver);
Vector3 rootPosition = solver.GetNode(spine[rootNodeIndex].chainIndex, spine[rootNodeIndex].nodeIndex).solverPosition;
Vector3 lastPosition = spine[spine.Length - 1].GetPlanePosition(solver);
// If we have more than 3 bones, use the FABRIK algorithm
if (useFABRIK) {
Vector3 offset = solver.GetNode(spine[rootNodeIndex].chainIndex, spine[rootNodeIndex].nodeIndex).solverPosition - spine[rootNodeIndex].transform.position;
for (int i = 0; i < spine.Length; i++) {
spine[i].ikPosition = spine[i].transform.position + offset;
}
// Iterating the FABRIK algorithm
for (int i = 0; i < iterations; i++) {
ForwardReach(lastPosition);
BackwardReach(firstPosition);
spine[rootNodeIndex].ikPosition = rootPosition;
}
} else {
// When we have just 3 bones, we know their positions already
spine[0].ikPosition = firstPosition;
spine[rootNodeIndex].ikPosition = rootPosition;
}
spine[spine.Length - 1].ikPosition = lastPosition;
// Mapping the spine bones to the solver
MapToSolverPositions(solver);
}
/*
* Rotation of plane nodes in the solver
* */
private Quaternion GetTargetRotation(IKSolverFullBody solver) {
Vector3 p1 = solver.GetNode(plane1ChainIndex, plane1NodeIndex).solverPosition;
Vector3 p2 = solver.GetNode(plane2ChainIndex, plane2NodeIndex).solverPosition;
Vector3 p3 = solver.GetNode(plane3ChainIndex, plane3NodeIndex).solverPosition;
if (p1 == p3) return Quaternion.identity;
return Quaternion.LookRotation(p2 - p1, p3 - p1);
//if (planeNode1.solverPosition == planeNode3.solverPosition) return Quaternion.identity;
//return Quaternion.LookRotation(planeNode2.solverPosition - planeNode1.solverPosition, planeNode3.solverPosition - planeNode1.solverPosition);
}
/*
* Gets the bone's position relative to its 3 plane nodes
* */
public Vector3 GetPlanePosition(IKSolverFullBody solver)
{
return(solver.GetNode(plane1ChainIndex, plane1NodeIndex).solverPosition + (GetTargetRotation(solver) * planePosition));
//return planeNode1.solverPosition + (targetRotation * planePosition);
}
/*
* Positioning and rotating the spine bones to match the solver positions
* */
private void MapToSolverPositions(IKSolverFullBody solver) {
// Translating the first bone
// Note: spine here also includes the pelvis
spine[0].SetToIKPosition();
spine[0].RotateToPlane(solver, 1f);
// Translating all the bones between the first and the last
for (int i = 1; i < spine.Length - 1; i++) {
spine[i].Swing(spine[i + 1].ikPosition, 1f);
if (twistWeight > 0) {
float bWeight = (float)i / ((float)spine.Length - 2);
Vector3 s1 = solver.GetNode(leftUpperArm.chainIndex, leftUpperArm.nodeIndex).solverPosition;
Vector3 s2 = solver.GetNode(rightUpperArm.chainIndex, rightUpperArm.nodeIndex).solverPosition;
spine[i].Twist(s1 - s2, spine[i + 1].ikPosition - spine[i].transform.position, bWeight * twistWeight);
}
}
// Translating the last bone
spine[spine.Length - 1].SetToIKPosition();
spine[spine.Length - 1].RotateToPlane(solver, 1f);
}
/*
* Computes the direction from the first node to the second node
* */
public Vector3 GetDir(IKSolverFullBody solver) {
if (!initiated) return Vector3.zero;
float w = weight * solver.IKPositionWeight;
// Apply the bend goal
if (bendGoal != null) {
Vector3 b = bendGoal.position - solver.GetNode(chainIndex1, nodeIndex1).solverPosition;
if (b != Vector3.zero) direction = b;
}
if (w >= 1f) return direction.normalized;
Vector3 solverDirection = solver.GetNode(chainIndex3, nodeIndex3).solverPosition - solver.GetNode(chainIndex1, nodeIndex1).solverPosition;
// Get rotation from animated limb direction to solver limb direction
Quaternion f = Quaternion.FromToRotation(bone3.position - bone1.position, solverDirection);
// Rotate the default bend direction by f
Vector3 dir = f * (bone2.position - bone1.position);
// Effector rotation
if (solver.GetNode(chainIndex3, nodeIndex3).effectorRotationWeight > 0f) {
// Bend direction according to the effector rotation
Vector3 effectorDirection = -Vector3.Cross(solverDirection, solver.GetNode(chainIndex3, nodeIndex3).solverRotation * defaultChildDirection);
dir = Vector3.Lerp(dir, effectorDirection, solver.GetNode(chainIndex3, nodeIndex3).effectorRotationWeight);
}
// Rotation Offset
if (rotationOffset != Quaternion.identity) {
Quaternion toOrtho = Quaternion.FromToRotation(rotationOffset * solverDirection, solverDirection);
dir = toOrtho * rotationOffset * dir;
}
if (w <= 0f) return dir;
return Vector3.Lerp(dir, direction.normalized, w);
}
/*
* Ortho-Normalize a vector to the first bone direction
* */
private Vector3 OrthoToBone1(IKSolverFullBody solver, Vector3 tangent) {
Vector3 normal = solver.GetNode(chainIndex2, nodeIndex2).solverPosition - solver.GetNode(chainIndex1, nodeIndex1).solverPosition;
Vector3.OrthoNormalize(ref normal, ref tangent);
return tangent;
}
public void WritePose(IKSolverFullBody solver, bool fullBody) {
if (weight <= 0f) return;
// Swing the parent bone to look at the first node's position
if (fullBody) {
if (parentBone != null) {
boneMapParent.Swing(solver.GetNode(boneMap1.chainIndex, boneMap1.nodeIndex).solverPosition, weight);
//boneMapParent.Swing(boneMap1.node.solverPosition, weight);
}
// Fix the first bone to it's node
//boneMap1.FixToNode(weight);
}
// Rotate the 2 first bones to the plane points
boneMap1.RotateToPlane(solver, weight);
boneMap2.RotateToPlane(solver, weight);
// Rotate the third bone to the rotation it had before solving
boneMap3.RotateToMaintain(maintainRotationWeight * weight * solver.IKPositionWeight);
// Rotate the third bone to the effector rotation
boneMap3.RotateToEffector(solver, weight);
}
/*
* Gets the bone's position relative to it's 3 plane nodes
* 限制骨骼在平面节点之间
* */
public Vector3 GetPlanePosition(IKSolverFullBody solver)
{
//计算 当前节点在相对于平面节点的方向需要做的位移偏移量 然后叠加到 solverPosition中
return(solver.GetNode(plane1ChainIndex, plane1NodeIndex).solverPosition + (GetTargetRotation(solver) * planePosition));
//return planeNode1.solverPosition + (targetRotation * planePosition);
}
public Vector3 GetPlanePosition(IKSolverFullBody solver)
{
return(solver.GetNode(this.plane1ChainIndex, this.plane1NodeIndex).solverPosition + this.GetTargetRotation(solver) * this.planePosition);
}
/*
* Gets the bone's position relative to it's 3 plane nodes
* */
public Vector3 GetPlanePosition(IKSolverFullBody solver) {
return solver.GetNode(plane1ChainIndex, plane1NodeIndex).solverPosition + (GetTargetRotation(solver) * planePosition);
//return planeNode1.solverPosition + (targetRotation * planePosition);
}
/*
* Gets the starting position of the iteration
* */
private Vector3 GetPosition(IKSolverFullBody solver, out Quaternion planeRotationOffset) {
planeRotationOffset = Quaternion.identity;
if (!isEndEffector) return solver.GetNode(chainIndex, nodeIndex).solverPosition; // non end-effectors are always free
if (maintainRelativePositionWeight <= 0f) return animatedPosition;
// Maintain relative position
Vector3 p = bone.position;
Vector3 dir = p - planeBone1.position;
planeRotationOffset = GetPlaneRotation(solver) * Quaternion.Inverse(animatedPlaneRotation);
p = solver.GetNode(plane1ChainIndex, plane1NodeIndex).solverPosition + planeRotationOffset * dir;
// Interpolate the rotation offset
planeRotationOffset = Quaternion.Lerp(Quaternion.identity, planeRotationOffset, maintainRelativePositionWeight);
return Vector3.Lerp(animatedPosition, p + solver.GetNode(chainIndex, nodeIndex).offset, maintainRelativePositionWeight);
}
