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