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); } } } } }