public void InitFromData(List <float> impulses, List <float> arcSteps,
List <float> curvature, List <float> torsion,
OrthonormalFrame initialFrame, Vector3 initialPos)
{
if (initialFrame.T.magnitude < 0.01f ||
initialFrame.B.magnitude < 0.01f ||
initialFrame.N.magnitude < 0.01f)
{
throw new System.Exception("Initial frame has zeroes");
}
displayPoints = new List <Vector3>();
lRenderer = GetComponent <LineRenderer>();
segments = new List <CurveSegment>();
// Create the initial segment
CurveSegment prevHelix = new CurveSegment(initialPos, curvature[0], -torsion[0], 0, arcSteps[0], initialFrame);
segments.Add(prevHelix);
AddPointsOfSegment(prevHelix);
float len = arcSteps[0];
// For each impulse, make a new segment rotated by that angle.
for (int i = 1; i < impulses.Count; i++)
{
float impulse = impulses[i];
float arcStep = arcSteps[i];
// New start point is the end of the previous curve segment.
Vector3 newBase = TransformedHelixPoint(prevHelix, prevHelix.arcLength);
// New frame is the frame rotated to the end of the segment.
OrthonormalFrame newFrame = TransformedHelixFrame(prevHelix, prevHelix.arcLength);
// Apply the torsion impulse as well.
Quaternion impulseRot = Quaternion.AngleAxis(Mathf.Rad2Deg * impulse, newFrame.T);
newFrame = newFrame.RotatedBy(impulseRot);
int clampedC = Mathf.Min(i, curvature.Count - 1);
int clampedT = Mathf.Min(i, torsion.Count - 1);
prevHelix = new CurveSegment(newBase, curvature[clampedC], -torsion[clampedT], impulse, arcStep, newFrame);
len += arcStep;
AddPointsOfSegment(prevHelix);
segments.Add(prevHelix);
}
// Add the last point of the last curve
displayPoints.Add(TransformedHelixPoint(prevHelix, arcSteps[arcSteps.Count - 1]));
// Set up line renderer
lRenderer.SetVertexCount(displayPoints.Count);
lRenderer.SetPositions(displayPoints.ToArray());
lRenderer.SetWidth(0.1f, 0.1f);
//Debug.Log("Arc length of TIC = " + ArcLength);
}
public OrthonormalFrame PropagateBishop(Vector3 prevPos, Vector3 nextPos, OrthonormalFrame prevFrame)
{
Vector3 tangent = (nextPos - position).normalized;
Vector3 prevTangent = (position - prevPos).normalized;
Vector3 binormal = Vector3.Cross(prevTangent, tangent).normalized;
float angle = TelescopeUtils.AngleBetween(prevTangent, tangent, binormal);
Quaternion rotation = Quaternion.AngleAxis(angle, binormal);
OrthonormalFrame rotated = prevFrame.RotatedBy(rotation);
bishopFrame = rotated;
return(rotated);
}
public static OrthonormalFrame FrameAlongHelix(float curvature, float torsion, float arcLength)
{
// No torsion = just a circular rotation.
if (Mathf.Abs(torsion) < 1e-6)
{
OrthonormalFrame defaultFrame = new OrthonormalFrame(Vector3.forward, Vector3.up,
Vector3.Cross(Vector3.forward, Vector3.up));
Quaternion r = rotateAlongCircle(curvature, arcLength);
return(defaultFrame.RotatedBy(r));
}
// Torsion but no curvature = rotate about forward axis in a screw motion
if (curvature < 1e-6)
{
OrthonormalFrame defaultFrame = new OrthonormalFrame(Vector3.forward, Vector3.up,
Vector3.Cross(Vector3.forward, Vector3.up));
float rotationAngle = torsion * arcLength;
Quaternion r = Quaternion.AngleAxis(Mathf.Rad2Deg * rotationAngle, Vector3.forward);
return(defaultFrame.RotatedBy(r));
}
float sumSq = curvature * curvature + torsion * torsion;
float a = curvature / sumSq;
float b = torsion / sumSq;
float abSqrt = Mathf.Sqrt(a * a + b * b);
float t = arcLength;
Vector3 tangent = new Vector3(b,
-a * Mathf.Sin(t / abSqrt),
a * Mathf.Cos(t / abSqrt)) / abSqrt;
tangent.y *= -1;
tangent.Normalize();
Vector3 normal = new Vector3(0,
Mathf.Cos(t / abSqrt),
Mathf.Sin(t / abSqrt)) * -1;
normal.y *= -1;
normal.Normalize();
Vector3 binormal = Vector3.Cross(tangent, normal);
return(new OrthonormalFrame(tangent, normal, binormal));
}
/// <summary>
/// Rotate this entire curve by the given rotation.
/// </summary>
/// <param name="rotation"></param>
public void Rotate(Quaternion rotation)
{
OrthonormalFrame initFrame = segments[0].frame.RotatedBy(rotation);
CurveSegment initSegment = segments[0];
initSegment.frame = initFrame;
segments[0] = initSegment;
CurveSegment prevHelix = segments[0];
float len = prevHelix.arcLength;
displayPoints.Clear();
AddPointsOfSegment(prevHelix);
for (int i = 1; i < segments.Count; i++)
{
// New start point is the end of the previous curve segment.
Vector3 newBase = TransformedHelixPoint(prevHelix, prevHelix.arcLength);
// New frame is the frame rotated to the end of the segment.
OrthonormalFrame newFrame = TransformedHelixFrame(prevHelix, prevHelix.arcLength);
// Apply the torsion impulse as well.
Quaternion impulseRot = Quaternion.AngleAxis(Mathf.Rad2Deg * segments[i].impulse, newFrame.T);
newFrame = newFrame.RotatedBy(impulseRot);
prevHelix = new CurveSegment(newBase, segments[i].curvature, segments[i].torsion,
segments[i].impulse, segments[i].arcLength, newFrame);
len += segments[i].arcLength;
AddPointsOfSegment(prevHelix);
segments[i] = prevHelix;
}
// Add the last point of the last curve
displayPoints.Add(TransformedHelixPoint(prevHelix, prevHelix.arcLength));
// Set up line renderer
lRenderer.SetVertexCount(displayPoints.Count);
lRenderer.SetPositions(displayPoints.ToArray());
lRenderer.SetWidth(0.1f, 0.1f);
}
void FixFrenetForward(int start)
{
int validIndex = start + 1;
// Search forward until we find a valid Frenet frame
while (discretizedPoints[validIndex].frenetFrame.B.magnitude == 0)
{
validIndex++;
}
// Un-rotate the valid frame so that it's aligned with the current tangent
OrthonormalFrame nextFrame = discretizedPoints[validIndex].frenetFrame;
Vector3 nextTangent = nextFrame.T;
Vector3 currentTangent = discretizedPoints[validIndex - 1].frenetFrame.T;
Quaternion reverseRotation = Quaternion.FromToRotation(nextTangent, currentTangent);
OrthonormalFrame backFrame = nextFrame.RotatedBy(reverseRotation);
// Set the aligned frame to all of the points missing frames
for (int i = start; i < validIndex; i++)
{
discretizedPoints[i].frenetFrame = backFrame;
}
}
public void RecomputeAndRedraw()
{
displayPoints.Clear();
AddPointsOfSegment(segments[0]);
// For each impulse, recompute the curve along that segment.
for (int i = 1; i < segments.Count; i++)
{
CurveSegment prevHelix = segments[i - 1];
float impulse = segments[i].impulse;
// New start point is the end of the previous curve segment.
Vector3 newBase = TransformedHelixPoint(prevHelix, prevHelix.arcLength);
// New frame is the frame rotated to the end of the segment.
OrthonormalFrame newFrame = TransformedHelixFrame(prevHelix, prevHelix.arcLength);
CurveSegment s = segments[i];
s.frame = newFrame;
s.startPosition = newBase;
segments[i] = s;
// Apply the torsion impulse as well.
Quaternion impulseRot = Quaternion.AngleAxis(Mathf.Rad2Deg * impulse, newFrame.T);
newFrame = newFrame.RotatedBy(impulseRot);
AddPointsOfSegment(segments[i]);
}
// Add the last point of the last curve
int last = segments.Count - 1;
displayPoints.Add(TransformedHelixPoint(segments[last], segments[last].arcLength));
// Set up line renderer
lRenderer.SetVertexCount(displayPoints.Count);
lRenderer.SetPositions(displayPoints.ToArray());
lRenderer.SetWidth(0.1f, 0.1f);
}
