private void Apply(HandState[] states)
{
for (int p = 0; p < points.Length; p++)
{
IKPoint pt = points[p];
if (pt.outputH != -1)
{
states[pt.outputH].points[pt.outputP] = pt.pos;
}
}
}
private void SolveStep()
{
for (int p = 0; p < points.Length; p++)
{
points[p].BeginForward();
}
for (int fr = 0; fr < forwardRoots.Length; fr++)
{
IKRoot fwd = forwardRoots[fr];
points[fwd.p].pos = fwd.pos;
}
//fabrik forward, repeat till we have found nothing to do
bool done = false;
while (!done)
{
done = true;
for (int p = 0; p < points.Length; p++)
{
IKPoint pt = points[p];
if (pt.propogatedToSiblings.Count < pt.siblingDistance.Count)
{
done = false;
//all children have reported:
if (pt.childAccumulator.Count() == pt.childDistance.Count)
{
foreach (var kv in pt.siblingDistance)
{
int s = kv.Key;
if (!pt.propogatedToSiblings.Contains(s))
{
IKPoint sibling = points[s];
//all its children have reported too
if (sibling.childAccumulator.Count() == sibling.childDistance.Count)
{
Vector3 us = pt.GetChildAccumulation();
Vector3 them = sibling.GetChildAccumulation();
Vector3 us2them = them - us;
Vector3 midpoint = 0.5f * (us + them);
//add our thing to their accumulator
sibling.siblingAccumulator.Add(midpoint + (0.5f * kv.Value * us2them.normalized));
pt.propogatedToSiblings.Add(s);
}
}
}
}
}
if (!pt.propogatedToParent)
{
done = false;
//all children and siblings have reported
if (pt.childAccumulator.Count() == pt.childDistance.Count && pt.siblingAccumulator.Count() == pt.siblingDistance.Count)
{
pt.propogatedToParent = true;
if (pt.parent != -1)
{
IKPoint parent = points[pt.parent];
//update pos except if we are the root
pt.pos = pt.GetSiblingAccumulation();
Vector3 us2them = parent.pos - pt.pos;
parent.childAccumulator.Add(pt.pos + (pt.parentDistance * us2them.normalized));
}
}
}
}
}
//do finger constraints - move tip into plane defined by joints
for (int h = 0; h < 2; h++)
{
int idx = pidTranslation[h, HandState.POINT_KNUCKLE[0]];
if (idx == -1)
{
continue;
}
for (int f = 0; f < 5; f++)
{
Vector3 knuckle = points[pidTranslation[h, HandState.POINT_KNUCKLE[f]]].pos;
Vector3 v1 = points[pidTranslation[h, HandState.POINT_LOJOINT[f]]].pos - knuckle;
Vector3 v2 = points[pidTranslation[h, HandState.POINT_TIP[f]]].pos - knuckle;
Vector3 pn = Vector3.Cross(v1.normalized, v2.normalized);
if (pn.sqrMagnitude < 0.001)
{
fingerPlanes[h, f] = Vector3.zero;
continue;
}
fingerPlanes[h, f] = pn.normalized;
}
}
//do backward
done = false;
while (!done)
{
done = true;
for (int p = 0; p < points.Length; p++)
{
IKPoint pt = points[p];
if (!pt.didBackward)
{
done = false;
IKPoint parent = points[pt.parent];
if (parent.didBackward)
{
Vector3 planeConstraint = Vector3.zero;
if (pt.outputH != -1 && pt.outputP != -1 && pt.outputP < 20 && pt.outputP % 4 > 0)
{
planeConstraint = fingerPlanes[pt.outputH, pt.outputP / 4];
}
Vector3 them2us = pt.pos - parent.pos;
if (planeConstraint.sqrMagnitude > 0.99f)
{
Debug.Assert(planeConstraint.sqrMagnitude < 1.01f);
them2us -= Vector3.Dot(them2us, planeConstraint) * planeConstraint;
}
pt.pos = parent.pos + (pt.parentDistance * them2us.normalized);
pt.didBackward = true;
}
}
}
}
}
IKRoot[] forwardRoots; //world space constraints
private FABRIK(HandState[] states, IKPointTarget[] pointTargets, IKGapTarget[] gapTargets)
{
int activeHands = (states[0].active ? 1 : 0) + (states[1].active ? 1 : 0);
Debug.Assert(activeHands > 0);
int totalPoints = activeHands * HandState.NUM_POINTS + pointTargets.Length + gapTargets.Length * 2;
points = new IKPoint[totalPoints];
pidTranslation = new int[2, HandState.NUM_POINTS];
fingerPlanes = new Vector3[2, 5];
for (int h = 0; h < 2; h++)
{
for (int ip = 0; ip < HandState.NUM_POINTS; ip++)
{
pidTranslation[h, ip] = -1;
}
for (int f = 0; f < 5; f++)
{
fingerPlanes[h, f] = Vector3.zero;
}
}
int p = 0;
for (int h = 0; h < 2; h++)
{
HandState state = states[h];
if (state.active)
{
for (int ip = 0; ip < HandState.NUM_POINTS; ip++)
{
IKPoint pt = points[p] = new IKPoint();
pidTranslation[h, ip] = p;
pt.pos = state.points[ip];
pt.outputH = h;
pt.outputP = ip;
p++;
}
for (int ip = 0; ip < HandState.NUM_POINTS; ip++)
{
if (HandState.POINT_PARENT[ip] != -1)
{
points[pidTranslation[h, ip]].parent = pidTranslation[h, HandState.POINT_PARENT[ip]];
}
}
}
}
forwardRoots = new IKRoot[pointTargets.Length];
for (int i = 0; i < pointTargets.Length; i++)
{
IKPointTarget targ = pointTargets[i];
Debug.Assert(states[targ.h].active);
forwardRoots[i] = new IKRoot(p, targ.worldTargetPos);
//anchor for the root
IKPoint pt = points[p] = new IKPoint();
pt.parent = pidTranslation[targ.h, HandState.POINT_PARENT[targ.b]];
//put it in the local pos for initial distance constraints, the roots will move it later
pt.pos = states[targ.h].LocalToWorld(targ.b, targ.localPos);
p++;
}
for (int i = 0; i < gapTargets.Length; i++)
{
IKGapTarget targ = gapTargets[i];
Debug.Assert(states[targ.h1].active);
Debug.Assert(states[targ.h2].active);
//anchor for the root
IKPoint pt = points[p] = new IKPoint();
pt.parent = pidTranslation[targ.h1, HandState.POINT_PARENT[targ.b1]];
pt.pos = states[targ.h1].LocalToWorld(targ.b1, targ.localPos1);
pt.siblingDistance[p + 1] = targ.dist;
p++;
pt = points[p] = new IKPoint();
pt.parent = pidTranslation[targ.h2, HandState.POINT_PARENT[targ.b2]];
pt.pos = states[targ.h2].LocalToWorld(targ.b2, targ.localPos2);
pt.siblingDistance[p - 1] = targ.dist;
p++;
}
Debug.Assert(p == totalPoints);
//setup parent/child lengths
for (p = 0; p < totalPoints; p++)
{
IKPoint pt = points[p];
if (pt.parent != -1)
{
IKPoint parent = points[pt.parent];
pt.parentDistance = Vector3.Distance(pt.pos, parent.pos);
parent.childDistance.Add(p, pt.parentDistance);
}
}
//setup sibling lengths
for (p = 0; p < totalPoints; p++)
{
IKPoint pt = points[p];
if (pt.parent != -1)
{
IKPoint parent = points[pt.parent];
foreach (var kv in parent.childDistance)
{
int sibling = kv.Key;
if (sibling != p)
{
pt.siblingDistance[sibling] = Vector3.Distance(pt.pos, points[sibling].pos);
}
}
}
}
}
