public Vector3 GetNormal(double t, Vector3 up) { double startT, endT; FindParallelSegmentTimes(t, _parallelAngle, up, out startT, out endT); Vector3 startUp, endUp; if (startT == 0.0 && !HasAcceptableAngle(startT, _parallelAngle, up)) { startUp = GetUpVector(startT).normalized; } else { startUp = _childPath.GetNormal(startT, up); } if (endT == 1.0 && !HasAcceptableAngle(endT, _parallelAngle, up)) { endUp = GetUpVector(endT).normalized; } else { endUp = _childPath.GetNormal(endT, up); } Vector3 startTangent = GetTangent(startT); var startMidUp = Quaternion.Inverse(Quaternion.LookRotation(startTangent, GetNormal(startT))) * Quaternion.LookRotation(startTangent, startUp); Vector3 endTangent = GetTangent(endT); var endMidUp = Quaternion.Inverse(Quaternion.LookRotation(endTangent, GetNormal(endT))) * Quaternion.LookRotation(endTangent, endUp); float normalisedT = Mathf.InverseLerp((float)startT, (float)endT, (float)t); Quaternion rot = QuaternionUtils.SlerpNoInvert(startMidUp, endMidUp, normalisedT); Vector3 tangent = GetTangent(endT); Quaternion baseRot = Quaternion.LookRotation(tangent, GetNormal(t)); Vector3 interimUp = (baseRot * rot * Vector3.up).normalized; return(_childPath.GetNormal(t, interimUp)); }
private Quaternion GetRotationQuat(double t) { t = GetMappedTime(t); // Find the start/end of the sample interval. int index = GetIndex(t); Sample sample = _samples[index]; // Lerp between the samples double progress = (t - sample.StartTime) / (sample.EndTime - sample.StartTime); if (index > 0 && index < _samples.Count - 1) { Sample prevUp = _samples[index - 1]; Sample nextUp = _samples[index + 1]; return(QuaternionUtils.Squad(prevUp.StartRotation, sample.StartRotation, sample.EndRotation, nextUp.EndRotation, (float)progress)); } else { return(QuaternionUtils.SlerpNoInvert(sample.StartRotation, sample.EndRotation, (float)progress)); } }
private void CalculateSamples(Vector3 startUp, Vector3 endUp) { const int SubIterCount = 8; _samples = CreateSamples(); Vector3 startTangent = GetTangent(GetUnmappedTime(0.0)); _correctedStartUp = Vector3.Cross(startTangent, Vector3.Cross(startUp, startTangent)).normalized; Vector3 lastTangent = startTangent; // Forward pass, calculate the change in rotation by iterating through path at set sample points. Quaternion lastQuat = Quaternion.identity; for (int i = 0; i < _samples.Count; ++i) { Sample sample = _samples[i]; double startTime = i == 0? 0.0 : _samples[i - 1].StartTime; double endTime = _samples[i].StartTime; for (int j = 0; j < SubIterCount; ++j) { double t = (j / (double)SubIterCount) * (endTime - startTime) + startTime; Vector3 currentTangent = _childPath.GetTangent(GetUnmappedTime(t)); // Calculate the rotation change between the last tangent and the current one. lastQuat = lastQuat * QuaternionUtils.FromToRotation(lastTangent, currentTangent); lastTangent = currentTangent; } sample.StartRotation = lastQuat; _samples[i] = sample; } // Check if there is a predefined end up vector to work towards. if (endUp.magnitude != 0f) { // Recalcute the end up vector so that it is tangential. Vector3 endTangent = GetTangent(GetUnmappedTime(1.0)); var correctedEndUp = Vector3.Cross(endTangent, Vector3.Cross(endUp, endTangent)).normalized; // Find the rotation which get's the final sample rotation to align with our correct up vector; lastQuat = QuaternionUtils.FromToRotation(_samples[_samples.Count - 1].StartRotation * _correctedStartUp, correctedEndUp); for (int i = _samples.Count - 1; i >= 0; --i) { Sample sample = _samples[i]; double t = sample.StartTime; // Apply the correction rotation gradually across the path. sample.StartRotation = QuaternionUtils.SlerpNoInvert(_samples[i].StartRotation, lastQuat * _samples[i].StartRotation, (float)t); _samples[i] = sample; } } // Update the endpoints for (int i = 0; i < _samples.Count - 1; ++i) { int nextIndex = i + 1; var sample = _samples[i]; sample.EndTime = _samples[nextIndex].StartTime; sample.EndRotation = _samples[nextIndex].StartRotation; _samples[i] = sample; } _samples.RemoveAt(_samples.Count - 1); }