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