/*
* Solving stage 1 of the FABRIK algorithm from end effectors towards the root.
* */
public void Stage1(FABRIKChain[] chain)
{
// Solving children first
for (int i = 0; i < children.Length; i++) chain[children[i]].Stage1(chain);
// The last chains
if (children.Length == 0) {
ik.solver.SolveForward(ik.solver.GetIKPosition());
return;
}
// Finding the centroid of child root solver positions
position = ik.solver.GetIKPosition();
Vector3 centroid = position;
float pullSum = 0f;
for (int i = 0; i < children.Length; i++) pullSum += chain[children[i]].pull;
for (int i = 0; i < children.Length; i++) {
if (chain[children[i]].children.Length == 0) chain[children[i]].ik.solver.SolveForward(chain[children[i]].ik.solver.GetIKPosition());
if (pullSum > 0) centroid += (chain[children[i]].ik.solver.bones[0].solverPosition - position) * (chain[children[i]].pull / Mathf.Clamp(pullSum, 1f, pullSum));
}
// Solve this chain forward
ik.solver.SolveForward(Vector3.Lerp(centroid, position, pin));
}
private static void AddSceneChain(FABRIKChain[] chain, Color color, FABRIKChain selected) {
foreach (FABRIKChain c in chain) {
col = GetChainColor(c, color);
IKSolverHeuristicInspector.AddScene(c.ik.solver as IKSolverHeuristic, col, selected == c);
}
}
// Calculate the centroid of child positions
private Vector3 GetCentroid(FABRIKChain[] chain) {
Vector3 position = ik.solver.GetIKPosition();
// The chain is pinned, ignore the children
if (pin >= 1f) return position;
// Get the sum of the pull values of all the children
float pullSum = 0f;
for (int i = 0; i < children.Length; i++) pullSum += chain[children[i]].pull;
// All pull values are zero
if (pullSum <= 0f) return position;
if (pullSum < 1f) pullSum = 1f;
// Calculating the centroid
Vector3 centroid = position;
for (int i = 0; i < children.Length; i++) {
// Vector from IKPosition to the first bone of the child
Vector3 toChild = chain[children[i]].ik.solver.bones[0].solverPosition - position;
// Weight of the child
float childWeight = chain[children[i]].pull / pullSum;
// Adding to the centroid
centroid += toChild * childWeight;
}
// No pinning
if (pin <= 0f) return centroid;
// Pinning
return centroid + (position - centroid) * pin;
}
private static void AddSceneChain(FABRIKChain chain, Color color, FABRIKChain selected)
{
col = GetChainColor(chain, color);
IKSolverHeuristicInspector.AddScene(chain.ik.solver as IKSolverHeuristic, col, selected == chain, sizeMlp);
foreach (FABRIKChain c in chain.children) AddSceneChain(c, color, selected);
}
private static Color GetChainColor(FABRIKChain chain, Color color) {
float midWeight = chain.pin;
midColor = new Color(Mathf.Lerp(1f, color.r, midWeight), Mathf.Lerp(1f, color.g, midWeight), Mathf.Lerp(1f, color.b, midWeight), Mathf.Lerp(0.5f, 1f, midWeight));
float endWeight = chain.pull;
endColor = new Color(Mathf.Lerp(1f, color.r, endWeight), Mathf.Lerp(0f, color.g, endWeight), Mathf.Lerp(0f, color.b, endWeight), Mathf.Lerp(0.5f, 1f, endWeight));
return chain.children.Length == 0? endColor: midColor;
}
/*
* Solving stage 2 of the FABRIK algoright from the root to the end effectors.
* */
public void Stage2(Vector3 rootPosition, FABRIKChain[] chain) {
// Solve this chain backwards
ik.solver.SolveBackward(rootPosition);
// Solve child chains
for (int i = 0; i < children.Length; i++) {
chain[children[i]].Stage2(ik.solver.bones[ik.solver.bones.Length - 1].transform.position, chain);
}
}
private static void AddSceneChain(FABRIKChain[] chain, Color color, FABRIKChain selected)
{
foreach (FABRIKChain c in chain)
{
col = GetChainColor(c, color);
IKSolverHeuristicInspector.AddScene(c.ik.solver as IKSolverHeuristic, col, selected == c, sizeMlp);
}
}
private static void SelectChain(FABRIKChain[] chain, ref FABRIKChain selected, Color color) {
foreach (FABRIKChain c in chain) {
if (c.ik.solver.IKPositionWeight > 0 && selected != c) {
Handles.color = GetChainColor(c, color);
if (Handles.Button(c.ik.solver.GetIKPosition(), Quaternion.identity, GetHandleSize(c.ik.solver.GetIKPosition()), GetHandleSize(c.ik.solver.GetIKPosition()), Handles.DotCap)) {
selected = c;
return;
}
}
}
}
private void AddPointsToArray(ref IKSolver.Point[] array, FABRIKChain chain)
{
IKSolver.Point[] points = chain.ik.solver.GetPoints();
Array.Resize <IKSolver.Point>(ref array, array.Length + points.Length);
int num = 0;
for (int i = array.Length - points.Length; i < array.Length; i++)
{
array[i] = points[num];
num++;
}
}
private static Color GetChainColor(FABRIKChain chain, Color color)
{
float midWeight = chain.pin;
midColor = new Color(Mathf.Lerp(1f, color.r, midWeight), Mathf.Lerp(1f, color.g, midWeight), Mathf.Lerp(1f, color.b, midWeight), Mathf.Lerp(0.5f, 1f, midWeight));
float endWeight = chain.pull;
endColor = new Color(Mathf.Lerp(1f, color.r, endWeight), Mathf.Lerp(0f, color.g, endWeight), Mathf.Lerp(0f, color.b, endWeight), Mathf.Lerp(0.5f, 1f, endWeight));
return(chain.children.Length == 0? endColor: midColor);
}
/*
* Solving stage 1 of the FABRIK algorithm from end effectors towards the root.
* */
public void Stage1(FABRIKChain[] chain) {
// Solving children first
for (int i = 0; i < children.Length; i++) chain[children[i]].Stage1(chain);
// The last chains
if (children.Length == 0) {
ik.solver.SolveForward(ik.solver.GetIKPosition());
return;
}
ik.solver.SolveForward(GetCentroid(chain));
}
private void AddPointsToArray(ref IKSolver.Point[] array, FABRIKChain chain)
{
IKSolver.Point[] chainArray = chain.ik.solver.GetPoints();
Array.Resize(ref array, array.Length + chainArray.Length);
int a = 0;
for (int i = array.Length - chainArray.Length; i < array.Length; i++)
{
array[i] = chainArray[a];
a++;
}
}
private static void SelectChain(FABRIKChain[] chain, ref FABRIKChain selected, Color color)
{
foreach (FABRIKChain c in chain)
{
if (c.ik.solver.IKPositionWeight > 0 && selected != c)
{
Handles.color = GetChainColor(c, color);
if (Handles.Button(c.ik.solver.GetIKPosition(), Quaternion.identity, buttonSize * sizeMlp, buttonSize * sizeMlp, Handles.DotCap))
{
selected = c;
return;
}
}
}
}
private static void SelectChain(FABRIKChain[] chain, ref FABRIKChain selected, Color color)
{
foreach (FABRIKChain c in chain)
{
if (c.ik.solver.IKPositionWeight > 0 && selected != c)
{
Handles.color = GetChainColor(c, color);
if (Inspector.DotButton(c.ik.solver.GetIKPosition(), Quaternion.identity, GetHandleSize(c.ik.solver.GetIKPosition()), GetHandleSize(c.ik.solver.GetIKPosition())))
{
selected = c;
return;
}
}
}
}
private static void AddSceneChain(FABRIKChain[] chain, Color color, FABRIKChain selected)
{
foreach (FABRIKChain c in chain)
{
if (!c.IsValid(null))
{
return;
}
}
foreach (FABRIKChain c in chain)
{
col = GetChainColor(c, color);
IKSolverHeuristicInspector.AddScene(c.ik.solver as IKSolverHeuristic, col, selected == c);
}
}
/*
* Draws the scene view helpers for IKSolverFABRIKRoot
* */
public static void AddScene(IKSolverFABRIKRoot solver, Color color, bool modifiable, ref FABRIKChain selected)
{
// Protect from null reference errors
if (!solver.IsValid(false)) return;
Handles.color = color;
// Selecting solvers
if (Application.isPlaying) {
foreach (FABRIKChain chain in solver.chains) SelectChain(chain, ref selected, color);
}
foreach (FABRIKChain chain in solver.chains) AddSceneChain(chain, color, selected);
// Root pin
Handles.color = new Color(Mathf.Lerp(1f, color.r, solver.rootPin), Mathf.Lerp(1f, color.g, solver.rootPin), Mathf.Lerp(1f, color.b, solver.rootPin), Mathf.Lerp(0.5f, 1f, solver.rootPin));
if (solver.GetRoot() != null) {
Handles.DrawLine(solver.chains[0].ik.solver.bones[0].transform.position, solver.GetRoot().position);
Handles.CubeCap(0, solver.GetRoot().position, Quaternion.identity, buttonSize * sizeMlp);
}
}
/*
* Draws the scene view helpers for IKSolverFABRIKRoot
* */
public static void AddScene(IKSolverFABRIKRoot solver, Color color, bool modifiable, ref FABRIKChain selected) {
// Protect from null reference errors
if (Application.isPlaying && !solver.initiated) return;
if (!Application.isPlaying) {
string message = string.Empty;
if (!solver.IsValid(ref message)) return;
}
Handles.color = color;
// Selecting solvers
if (Application.isPlaying) {
SelectChain(solver.chains, ref selected, color);
}
AddSceneChain(solver.chains, color, selected);
// Root pin
Handles.color = new Color(Mathf.Lerp(1f, color.r, solver.rootPin), Mathf.Lerp(1f, color.g, solver.rootPin), Mathf.Lerp(1f, color.b, solver.rootPin), Mathf.Lerp(0.5f, 1f, solver.rootPin));
if (solver.GetRoot() != null) {
Handles.DrawLine(solver.chains[0].ik.solver.bones[0].transform.position, solver.GetRoot().position);
Handles.CubeCap(0, solver.GetRoot().position, Quaternion.identity, GetHandleSize(solver.GetRoot().position));
}
}
private void AddPointsToArray(ref IKSolver.Point[] array, FABRIKChain chain)
{
IKSolver.Point[] chainArray = chain.ik.solver.GetPoints();
Array.Resize(ref array, array.Length + chainArray.Length);
int a = 0;
for (int i = array.Length - chainArray.Length; i < array.Length; i++) {
array[i] = chainArray[a];
a++;
}
}
public override bool IsValid(bool log)
{
if (this.chains.Length == 0)
{
if (log)
{
base.LogWarning("IKSolverFABRIKRoot contains no chains.");
}
return(false);
}
FABRIKChain[] array = this.chains;
for (int i = 0; i < array.Length; i++)
{
FABRIKChain fABRIKChain = array[i];
if (log)
{
if (!fABRIKChain.IsValid(new Warning.Logger(base.LogWarning)))
{
return(false);
}
}
else if (!fABRIKChain.IsValid(null))
{
return(false);
}
}
for (int j = 0; j < this.chains.Length; j++)
{
for (int k = 0; k < this.chains.Length; k++)
{
if (j != k && this.chains[j].ik == this.chains[k].ik)
{
if (log)
{
base.LogWarning(this.chains[j].ik.name + " is represented more than once in IKSolverFABRIKRoot chain.");
}
return(false);
}
}
}
for (int l = 0; l < this.chains.Length; l++)
{
for (int m = 0; m < this.chains[l].children.Length; m++)
{
int num = this.chains[l].children[m];
if (num < 0)
{
if (log)
{
base.LogWarning(string.Concat(new object[]
{
this.chains[l].ik.name,
"IKSolverFABRIKRoot chain at index ",
l,
" has invalid children array. Child index is < 0."
}));
}
return(false);
}
if (num == l)
{
if (log)
{
base.LogWarning(string.Concat(new object[]
{
this.chains[l].ik.name,
"IKSolverFABRIKRoot chain at index ",
l,
" has invalid children array. Child index is referencing to itself."
}));
}
return(false);
}
if (num >= this.chains.Length)
{
if (log)
{
base.LogWarning(string.Concat(new object[]
{
this.chains[l].ik.name,
"IKSolverFABRIKRoot chain at index ",
l,
" has invalid children array. Child index > number of chains"
}));
}
return(false);
}
for (int n = 0; n < this.chains.Length; n++)
{
if (num == n)
{
for (int num2 = 0; num2 < this.chains[n].children.Length; num2++)
{
if (this.chains[n].children[num2] == l)
{
if (log)
{
base.LogWarning(string.Concat(new string[]
{
"Circular parenting. ",
this.chains[n].ik.name,
" already has ",
this.chains[l].ik.name,
" listed as it's child."
}));
}
return(false);
}
}
}
}
for (int num3 = 0; num3 < this.chains[l].children.Length; num3++)
{
if (m != num3 && this.chains[l].children[num3] == num)
{
if (log)
{
base.LogWarning(string.Concat(new object[]
{
"Chain number ",
num,
" is represented more than once in the children of ",
this.chains[l].ik.name
}));
}
return(false);
}
}
}
}
return(true);
}
/*
* Draws the scene view helpers for IKSolverFABRIKRoot
* */
public static void AddScene(IKSolverFABRIKRoot solver, Color color, bool modifiable, ref FABRIKChain selected)
{
// Protect from null reference errors
if (Application.isPlaying && !solver.initiated)
{
return;
}
if (!Application.isPlaying)
{
string message = string.Empty;
if (!solver.IsValid(ref message))
{
return;
}
}
Handles.color = color;
// Selecting solvers
if (Application.isPlaying)
{
SelectChain(solver.chains, ref selected, color);
}
AddSceneChain(solver.chains, color, selected);
// Root pin
Handles.color = new Color(Mathf.Lerp(1f, color.r, solver.rootPin), Mathf.Lerp(1f, color.g, solver.rootPin), Mathf.Lerp(1f, color.b, solver.rootPin), Mathf.Lerp(0.5f, 1f, solver.rootPin));
if (solver.GetRoot() != null)
{
Handles.DrawLine(solver.chains[0].ik.solver.bones[0].transform.position, solver.GetRoot().position);
Handles.CubeHandleCap(0, solver.GetRoot().position, Quaternion.identity, GetHandleSize(solver.GetRoot().position), EventType.Repaint);
}
}
private static void SelectChain(FABRIKChain chain, ref FABRIKChain selected, Color color)
{
if (chain.ik.solver.IKPositionWeight > 0 && selected != chain) {
Handles.color = GetChainColor(chain, color);
if (Handles.Button(chain.ik.solver.GetIKPosition(), Quaternion.identity, buttonSize * sizeMlp, buttonSize * sizeMlp, Handles.DotCap)) {
selected = chain;
return;
}
}
foreach (FABRIKChain c in chain.children) SelectChain(c, ref selected, color);
}
/*
* Draws the scene view helpers for IKSolverFABRIKRoot
* */
public static void AddScene(IKSolverFABRIKRoot solver, Color color, bool modifiable, ref FABRIKChain selected)
{
// Protect from null reference errors
if (!solver.IsValid(false))
{
return;
}
Handles.color = color;
// Selecting solvers
if (Application.isPlaying)
{
SelectChain(solver.chains, ref selected, color);
}
AddSceneChain(solver.chains, color, selected);
// Root pin
Handles.color = new Color(Mathf.Lerp(1f, color.r, solver.rootPin), Mathf.Lerp(1f, color.g, solver.rootPin), Mathf.Lerp(1f, color.b, solver.rootPin), Mathf.Lerp(0.5f, 1f, solver.rootPin));
if (solver.GetRoot() != null)
{
Handles.DrawLine(solver.chains[0].ik.solver.bones[0].transform.position, solver.GetRoot().position);
Handles.CubeCap(0, solver.GetRoot().position, Quaternion.identity, buttonSize * sizeMlp);
}
}
public override Point GetPoint(Transform transform) => default; // 0x00000001809B86A0-0x00000001809B8750
private void AddPointsToArray(ref Point[] array, FABRIKChain chain)
{
} // 0x00000001809B7FC0-0x00000001809B8130
