public static Quaternion RotateAlongHelix(float curvature, float torsion, float arcLength)
{
// No torsion = just a circular rotation.
if (Mathf.Abs(torsion) < 1e-6)
{
return(rotateAlongCircle(curvature, arcLength));
}
// Torsion but no curvature = rotate about forward axis in a screw motion
if (curvature < 1e-6)
{
float rotationAngle = torsion * arcLength;
return(Quaternion.AngleAxis(Mathf.Rad2Deg * rotationAngle, Vector3.forward));
}
// Correct because the initial tangent of a helix isn't (0, 0, 1),
// but we want it to be for linking up of helical pieces.
OrthonormalFrame zeroFrame = FrameAlongHelix(curvature, torsion, 0);
Quaternion correctiveR = Quaternion.Inverse(Quaternion.LookRotation(zeroFrame.T, zeroFrame.N));
OrthonormalFrame frame = FrameAlongHelix(curvature, torsion, arcLength);
// Corrective rotation so that initial tangent is forward.
//Quaternion r = Quaternion.FromToRotation(Vector3.forward, Vector3.down);
return(correctiveR * Quaternion.LookRotation(frame.T, frame.N));
}
TorsionImpulseCurve MakeTorsionImpulseCurve(List <float> impulses, List <float> arcPoints,
List <float> curvatureList, List <float> torsionList)
{
Debug.Log("Arc length of DC = " + ArcLength);
Vector3 startingNormal = Vector3.Cross(startingBinormal, startingTangent);
OrthonormalFrame startFrame = new OrthonormalFrame(startingTangent, startingNormal, startingBinormal);
List <float> arcSteps = new List <float>();
for (int i = 0; i < arcPoints.Count - 1; i++)
{
arcSteps.Add(arcPoints[i + 1] - arcPoints[i]);
}
GameObject obj = new GameObject();
obj.name = "TorsionApproxCurve";
TorsionImpulseCurve impulseCurve = obj.AddComponent <TorsionImpulseCurve>();
impulseCurve.InitFromData(impulses, arcSteps,
curvatureList, torsionList,
startFrame, StartingPoint);
impulseCurve.SetMaterial(lineRender.material);
return(impulseCurve);
}
TorsionImpulseCurve MakeTorsionImpulseCurve(List <float> impulses, List <float> arcPoints, float constTorsion)
{
float averageCurvature = AverageCurvature();
Vector3 startingNormal = Vector3.Cross(startingBinormal, startingTangent);
OrthonormalFrame startFrame = new OrthonormalFrame(startingTangent, startingNormal, startingBinormal);
List <float> arcSteps = new List <float>();
for (int i = 0; i < arcPoints.Count - 1; i++)
{
arcSteps.Add(arcPoints[i + 1] - arcPoints[i]);
}
GameObject obj = new GameObject();
obj.name = "TorsionApproxCurve";
TorsionImpulseCurve impulseCurve = obj.AddComponent <TorsionImpulseCurve>();
impulseCurve.InitFromData(impulses, arcSteps,
averageCurvature, constTorsion,
startFrame, StartingPoint);
impulseCurve.SetMaterial(lineRender.material);
impulseCurve.BulbShrinkRatio = ActualShrinkRatio;
return(impulseCurve);
}
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 CurveSegment(Vector3 basePos, float curv, float tors, float imp, float len, OrthonormalFrame f)
{
startPosition = basePos;
curvature = curv;
torsion = tors;
impulse = imp;
arcLength = len;
frame = f;
}
public void ComputeFrenet(Vector3 prevPos, Vector3 nextPos)
{
Vector3 tangent = (nextPos - position).normalized;
Vector3 prevTangent = (position - prevPos).normalized;
Vector3 binormal = Vector3.Cross(prevTangent, tangent).normalized;
Vector3 normal = Vector3.Cross(binormal, tangent);
frenetFrame = new OrthonormalFrame(tangent, normal, binormal);
}
OrthonormalFrame TransformedHelixFrame(CurveSegment cs, float arcLen)
{
// Apply the local helix rotation.
Quaternion oldRot = Quaternion.LookRotation(cs.frame.T, cs.frame.N);
Quaternion newRot = oldRot *
TelescopeUtils.RotateAlongHelix(cs.curvature, cs.torsion, arcLen)
* Quaternion.Inverse(oldRot);
OrthonormalFrame newFrame = cs.frame.RotatedBy(newRot);
return(newFrame);
}
public void InitFromData(List <float> impulses, List <float> arcSteps,
float curvature, float torsion,
OrthonormalFrame initialFrame, Vector3 initialPos)
{
List <float> c = new List <float>();
c.Add(curvature);
List <float> t = new List <float>();
t.Add(torsion);
InitFromData(impulses, arcSteps, c, t, initialFrame, initialPos);
}
void SetPositionOnCurve()
{
if (!dCurve)
{
return;
}
transform.position = dCurve.PositionAtPoint(arcLength);
OrthonormalFrame frame = bishop ? dCurve.BishopAtPoint(arcLength) : dCurve.FrenetAtPoint(arcLength);
Quaternion r = Quaternion.LookRotation(frame.T, frame.N);
transform.rotation = r;
}
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 OrthonormalFrame BishopAtPoint(float t)
{
int segNum;
float scaledT;
ScaleArcParameter(out segNum, out scaledT, t);
if (segNum == discretizedPoints.Count - 1)
{
return(discretizedPoints[segNum].bishopFrame);
}
return(OrthonormalFrame.Slerp(discretizedPoints[segNum].bishopFrame,
discretizedPoints[segNum + 1].bishopFrame,
scaledT));
}
public static OrthonormalFrame Slerp(OrthonormalFrame frame1, OrthonormalFrame frame2, float t)
{
Vector3 tangent1 = frame1.T;
Vector3 tangent2 = frame2.T;
Vector3 tangent = Vector3.Slerp(tangent1, tangent2, t);
Vector3 normal1 = frame1.N;
Vector3 normal2 = frame2.N;
Vector3 normal = Vector3.Slerp(normal1, normal2, t);
Vector3 binormal1 = frame1.B;
Vector3 binormal2 = frame2.B;
Vector3 binormal = Vector3.Slerp(binormal1, binormal2, t);
return(new OrthonormalFrame(tangent, normal, binormal));
}
void SetPositionOnCurve()
{
if (!dCurve)
{
return;
}
Vector3 p = dCurve.transform.position;
Quaternion frameR = dCurve.transform.rotation;
transform.position = frameR * dCurve.PositionAtPoint(arcLength) + p;
OrthonormalFrame frame = dCurve.FrameAtPoint(arcLength).RotatedBy(frameR);
Quaternion r = Quaternion.LookRotation(frame.T, frame.N);
transform.rotation = r;
}
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));
}
void FixFrenetBackward(int start)
{
// Find the most recent valid frame
int validIndex = start - 1;
while (discretizedPoints[validIndex].frenetFrame.B.magnitude == 0)
{
validIndex--;
}
OrthonormalFrame validFrame = discretizedPoints[validIndex].frenetFrame;
// No need to un-rotate because the tangents are already aligned.
for (int i = start; i > validIndex; i--)
{
discretizedPoints[i].frenetFrame = validFrame;
}
}
/// <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);
}
public static Vector3 TranslateAlongHelix(float curvature, float torsion, float arcLength)
{
if (Mathf.Abs(torsion) < 1e-6)
{
return(translateAlongCircle(curvature, arcLength));
}
if (curvature < 1e-6)
{
return(Vector3.forward * arcLength);
}
// Correct because the initial tangent of a helix isn't (0, 0, 1),
// but we want it to be for linking up of helical pieces.
OrthonormalFrame zeroFrame = FrameAlongHelix(curvature, torsion, 0);
Quaternion correctiveR = Quaternion.Inverse(Quaternion.LookRotation(zeroFrame.T, zeroFrame.N));
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 pos = new Vector3(b * t / abSqrt,
a * Mathf.Cos(t / abSqrt),
a * Mathf.Sin(t / abSqrt));
// This treats (0, 0, 0) as the center and (0, 0, a) as the first point.
// Want to treat (0, -a, 0) as the first point, so rotate.
//Quaternion r = Quaternion.FromToRotation(Vector3.forward, Vector3.down);
pos.y *= -1;
// Shift so that (0, a, 0) is the center and (0, 0, 0) is the first point.
pos += (a * Vector3.up);
return(correctiveR * pos);
}
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);
}
void AddAxes()
{
foreach (TelescopeShell shell in segment.shells)
{
GameObject axisObj = new GameObject();
TorsionImpulseCurve axis = axisObj.AddComponent <TorsionImpulseCurve>();
List <float> impulses = new List <float>();
impulses.Add(0);
List <float> arcs = new List <float>();
arcs.Add(shell.length);
OrthonormalFrame frame = new OrthonormalFrame(Vector3.forward, Vector3.up, Vector3.left);
axis.InitFromData(impulses, arcs, shell.curvature, -shell.torsion, frame, Vector3.zero);
LineRenderer axisLR = axis.GetComponent <LineRenderer>();
axisLR.useWorldSpace = false;
axisLR.material = lineMaterial;
axis.transform.parent = shell.transform;
axis.transform.localPosition = Vector3.zero;
axis.transform.localRotation = Quaternion.identity;
}
}
public void SetOrientation(OrthonormalFrame frame)
{
transform.rotation = Quaternion.LookRotation(frame.T, frame.B);
}
public TelescopeSegment MakeTelescope(float startRadius, bool reverse = false)
{
List <TelescopeParameters> tParams = new List <TelescopeParameters>();
CurveSegment initialSeg = (reverse) ? segments[segments.Count - 1] : segments[0];
float wallThickness = Constants.WALL_THICKNESS;
float currRadius = startRadius * BulbShrinkRatio;
TelescopeParameters initial = new TelescopeParameters(initialSeg.arcLength, currRadius,
Constants.WALL_THICKNESS, initialSeg.curvature, initialSeg.torsion, 0);
tParams.Add(initial);
for (int i = 1; i < segments.Count; i++)
{
int index = (reverse) ? segments.Count - 1 - i : i;
/*
* if (i == 2 && StartJuncture && StartJuncture.childCurves.Count == 5)
* {
* Debug.Log("Hack to create lizard toenails, please delete once done");
* currRadius -= 1.5f * wallThickness;
* }*/
// Subtract the wall thickness of the previous piece
currRadius -= wallThickness;
float curvature = (reverse) ? segments[index].curvature : segments[index].curvature;
float torsion = (reverse) ? segments[index].torsion : segments[index].torsion;
float impulse = (reverse) ? -segments[index + 1].impulse : -segments[index].impulse;
impulse *= Mathf.Rad2Deg;
TelescopeParameters p = new TelescopeParameters(segments[index].arcLength, currRadius,
wallThickness, curvature, torsion, impulse);
tParams.Add(p);
}
GameObject obj = new GameObject();
obj.name = "telescope" + (numScopes++);
TelescopeSegment segment = obj.AddComponent <TelescopeSegment>();
segment.paramMode = SegmentParametersMode.Concrete;
segment.material = DesignerController.instance.defaultTelescopeMaterial;
if (reverse)
{
CurveSegment lastSeg = segments[segments.Count - 1];
obj.transform.position = TransformedHelixPoint(lastSeg, lastSeg.arcLength);
OrthonormalFrame initFrame = TransformedHelixFrame(lastSeg, lastSeg.arcLength);
segment.initialDirection = -initFrame.T;
segment.initialUp = initFrame.N;
}
else
{
obj.transform.position = segments[0].startPosition;
segment.initialDirection = segments[0].frame.T;
segment.initialUp = segments[0].frame.N;
}
segment.MakeShellsFromConcrete(tParams);
if (reverse)
{
segment.ReverseTelescope();
}
return(segment);
}
public OrthonormalFrame PropagateBishop(DCurvePoint prev, DCurvePoint next, OrthonormalFrame prevFrame)
{
return(PropagateBishop(prev.position, next.position, prevFrame));
}
/// <summary>
/// Resolve points where the Frenet frame is undefined due to
/// the tangent being constant (i.e. zero derivative).
/// </summary>
void FixFrenetFrames()
{
// Find the index of the first valid frame.
int firstFrameIndex = -1;
for (int i = 0; i < discretizedPoints.Count; i++)
{
OrthonormalFrame frame = discretizedPoints[i].frenetFrame;
if (frame.B.magnitude > 0)
{
firstFrameIndex = i;
break;
}
}
// If there were no valid Frenet frames, then the entire curve is just
// a striaght line. We just assign an arbitrary
// frame aligned with the tangent for each one.
if (firstFrameIndex == -1)
{
Vector3 tangent = discretizedPoints[0].frenetFrame.T;
Vector3 normal, binormal;
// If the tangent is pointing up, use an orthogonal direction
if (tangent == Vector3.up)
{
binormal = Vector3.right;
normal = Vector3.Cross(binormal, tangent);
}
// Otherwise take some vector orthogonal to the tangent,
// by projecting the up direction onto it
else
{
binormal = Vector3.up;
binormal = binormal - Vector3.Dot(binormal, tangent) * tangent;
binormal.Normalize();
normal = Vector3.Cross(binormal, tangent);
}
startingBinormal = binormal;
OrthonormalFrame defaultFrame = new OrthonormalFrame(tangent, normal, binormal);
foreach (DCurvePoint dcp in discretizedPoints)
{
dcp.frenetFrame = defaultFrame;
}
}
// Otherwise there is at least one valid frame, so propagate to all missing frames.
else
{
for (int i = 0; i < discretizedPoints.Count; i++)
{
// If the current frame is invalid (has zero binormal), fix it
if (discretizedPoints[i].frenetFrame.B.magnitude == 0)
{
if (i < firstFrameIndex)
{
FixFrenetForward(i);
}
else if (i > firstFrameIndex)
{
FixFrenetBackward(i);
}
}
}
}
}
