protected override Quaternion[] UpdateElementImpl(bool useLocal)
{
if (Object)
{
Vector3 pole = _polePosition - Object.position;
var angles = Quaternion.LookRotation(Target.position - Object.position, pole).eulerAngles;
angles.z = 0;
Quaternion rot = Quaternion.Euler(angles);
if (useLocal)
{
rot = IKRigging_Utils.GetLocalRotation(Object, rot);
}
return(new Quaternion[] { rot });
}
return(new Quaternion[] { new Quaternion() });
}
protected override Quaternion[] UpdateElementImpl(bool useLocal)
{
if (_points.Count < 2)
{
throw new UnityException("Points must be at least two trasforms");
}
_wasUpdated = true;
var target = _target.position;
var begin = _points[0].position;
var pole = _pole != null ? _pole.position : begin + new Vector3(0, 1, 0);
if (target == _lastTargetPos && pole == _lastPolePos && _lastStartPos == begin)
{
return(_resultCache);
}
_lastTargetPos = target;
_lastPolePos = pole;
_lastStartPos = begin;
var planeToSpaceMat = Matrix4x4.TRS(begin, Quaternion.LookRotation(target - begin, pole - begin), new Vector3(1, 1, 1));
var spaceToPlaneMat = planeToSpaceMat.inverse;
var targetPlane = spaceToPlaneMat.MultiplyPoint(target);
double[] lengths = null;
Vector[] positionsPlane;
if (_points.Count >= 3)
{
_IK.Target = new Vector(targetPlane.z, targetPlane.y);
lengths = new double[_points.Count - 1];
for (int i = 1; i < _points.Count; i++)
{
lengths[i - 1] = (_points[i - 1].position - _points[i].position).magnitude;
}
_IK.Lengths = lengths;
//Profiler.BeginSample("InnerIK");
positionsPlane = _IK.GetPositions();
//Profiler.EndSample();
}
else //must be 2 then
{
lengths = new double[] { (_points[1].position - _points[0].position).magnitude };
positionsPlane = new Vector[] { new Vector(), new Vector(lengths[0], 0) };
}
if (positionsPlane == null)
{
throw new UnityException(string.Format("IK - acquired points were NULL\r\n legths:{0} \r\n target:{1}", string.Join(", ", lengths.Select(o => o.ToString()).ToArray()), targetPlane.z));
}
if (_positionsSpaceCache == null)
{
_positionsSpaceCache = new Vector3[positionsPlane.Length];
}
Vector3[] positionsSpace = _positionsSpaceCache;
for (int i = 0; i < positionsSpace.Length; i++)
{
var planePoint = positionsPlane[i];
positionsSpace[i] = planeToSpaceMat.MultiplyPoint(new Vector3(0, (float)planePoint.Y, (float)planePoint.X));
}
if (_resultCache == null)
{
_resultCache = new Quaternion[positionsSpace.Length - 1];
}
Quaternion[] rotations = _resultCache;
for (int i = 0; i < positionsSpace.Length - 1; i++)
{
var localPole = positionsSpace[(i + 2) % positionsSpace.Length] - positionsSpace[i + 1];
localPole.Normalize();
var localDir = positionsSpace[i + 1] - positionsSpace[i];
localDir.Normalize();
var localPoleQuality = Mathf.Min((localPole - localDir).sqrMagnitude, (localPole + localDir).sqrMagnitude);
if (localPoleQuality < 0.01)
{
localPole = -(pole - positionsSpace[i]);
}
rotations[i] = Quaternion.LookRotation(localDir, localPole);
}
//MonoBehaviour.print(string.Join("<->", _boundPoints.Select(o => o.gameObject.name).ToArray()));
//return rotations;
//MonoBehaviour.print("global:"+string.Join(";", rotations.Select(o => o.eulerAngles.ToString()).ToArray()));
if (useLocal)
{
rotations = IKRigging_Utils.GetLocalRotations(BoundObjects, rotations);
//MonoBehaviour.print("local:" + string.Join(";", rotations.Select(o => o.eulerAngles.ToString()).ToArray()));
}
_resultCache = rotations;
return(rotations);
}
