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