void ComputeBishopFrames()
{
if (discretizedPoints.Count <= 2)
{
return;
}
for (int i = 0; i < discretizedPoints.Count; i++)
{
DCurvePoint dcp = discretizedPoints[i];
if (i == 0)
{
dcp.bishopFrame = dcp.frenetFrame;
}
else if (i == discretizedPoints.Count - 1)
{
dcp.bishopFrame = discretizedPoints[i - 1].bishopFrame;
}
else
{
DCurvePoint prevPoint = discretizedPoints[i - 1];
DCurvePoint nextPoint = discretizedPoints[i + 1];
dcp.PropagateBishop(prevPoint, nextPoint, prevPoint.bishopFrame);
}
}
}
void ComputeFrenetFrames()
{
if (discretizedPoints.Count <= 2)
{
return;
}
for (int i = 0; i < discretizedPoints.Count; i++)
{
DCurvePoint dcp = discretizedPoints[i];
if (i == 0)
{
Vector3 prevPos = StartingPoint;
dcp.ComputeFrenet(prevPos, discretizedPoints[i + 1]);
}
else if (i == discretizedPoints.Count - 1)
{
Quaternion r = Quaternion.AngleAxis(discretizedPoints[i].bendingAngle,
discretizedPoints[i - 1].frenetFrame.B);
dcp.frenetFrame = discretizedPoints[i - 1].frenetFrame.RotatedBy(r);
}
else
{
DCurvePoint prevPoint = discretizedPoints[i - 1];
DCurvePoint nextPoint = discretizedPoints[i + 1];
dcp.ComputeFrenet(prevPoint, nextPoint);
}
}
FixFrenetFrames();
}
Vector3 ReconstructFromAngles()
{
curvePoints = new List <Vector3>();
Vector3 currentPoint = StartingPoint;
curvePoints.Add(currentPoint);
Vector3 currentDir = startingTangent;
currentPoint += currentDir * segmentLength;
curvePoints.Add(currentPoint);
Vector3 currentBinormal = startingBinormal;
// Rotate the direction about the current binormal by the given angle,
// and then offset to reach the next point.
for (int i = 0; i < discretizedPoints.Count; i++)
{
DCurvePoint dcp = discretizedPoints[i];
dcp.position = currentPoint;
currentBinormal = Quaternion.AngleAxis(dcp.twistingAngle, currentDir) * currentBinormal;
Quaternion nextRot = Quaternion.AngleAxis(dcp.bendingAngle, currentBinormal);
currentDir = nextRot * currentDir;
currentPoint += currentDir * segmentLength;
if (float.IsNaN(currentPoint.x))
{
Debug.Log("Binormal = " + currentBinormal);
throw new System.Exception("NaN");
}
curvePoints.Add(currentPoint);
}
SetupLineRenderer();
return(currentPoint);
}
public static float ToBendAngle(DCurvePoint dcp)
{
return(dcp.bendingAngle);
}
public static float ToTwistAngle(DCurvePoint dcp)
{
return(dcp.twistingAngle);
}
public OrthonormalFrame PropagateBishop(DCurvePoint prev, DCurvePoint next, OrthonormalFrame prevFrame)
{
return(PropagateBishop(prev.position, next.position, prevFrame));
}
public void ComputeFrenet(DCurvePoint prev, DCurvePoint next)
{
ComputeFrenet(prev.position, next.position);
}
public void ComputeFrenet(Vector3 prevPos, DCurvePoint next)
{
ComputeFrenet(prevPos, next.position);
}
// Compute all internal bending axes/angles from a list of points.
public void InitFromPoints(List <Vector3> points, float segLength)
{
segmentLength = segLength;
StartingPoint = points[0];
startingTangent = points[1] - points[0];
startingTangent.Normalize();
discretizedPoints = new List <DCurvePoint>();
Vector3 prevBinormal = Vector3.zero;
// We need to store bending angles / directions of all the interior
// vertices of the curve (but not the endpoints).
for (int i = 1; i < points.Count - 1; i++)
{
Vector3 previousVec = points[i] - points[i - 1];
Vector3 nextVec = points[i + 1] - points[i];
previousVec.Normalize();
nextVec.Normalize();
// If zero, then treat it as a straight segment
if (nextVec.magnitude < 0.5f)
{
DCurvePoint d = new DCurvePoint(prevBinormal, 0, 0);
discretizedPoints.Add(d);
continue;
}
Vector3 curvatureBinormal = Vector3.Cross(previousVec, nextVec).normalized;
if (i == 1)
{
startingBinormal = curvatureBinormal;
}
// Compute bending angles (discrete curvature).
float dot = Vector3.Dot(previousVec, nextVec);
float bendAngle = (dot >= 1) ? 0 : Mathf.Rad2Deg * Mathf.Acos(dot);
// Compute twisting angles (discrete torsion).
float twistAngle;
// Compute twist angles as we go along.
// The first vertex is considered to have no twist.
if (i == 1)
{
twistAngle = 0;
}
else
{
twistAngle = TelescopeUtils.AngleBetween(prevBinormal, curvatureBinormal, previousVec);
// If the bend angle is tiny, then the curve is basically straight, so
// just set twist values to 0 to avoid unnecessary twisting.
/*if (Mathf.Abs(bendAngle) <= 0.1)
* {
* bendAngle = 0;
* twistAngle = 0;
* }*/
}
if (float.IsNaN(bendAngle))
{
throw new System.Exception("Bend angle is nan, dot = " + dot);
}
if (float.IsNaN(twistAngle))
{
throw new System.Exception("Twist angle is nan");
}
prevBinormal = curvatureBinormal;
DCurvePoint dcp = new DCurvePoint(curvatureBinormal.normalized,
bendAngle, twistAngle);
discretizedPoints.Add(dcp);
}
if (startingBinormal.magnitude < 0.001f)
{
if (startingTangent == Vector3.up)
{
startingBinormal = Vector3.right;
}
else
{
startingBinormal = Vector3.up;
Vector3 orthogonal = Vector3.Dot(startingBinormal, startingTangent) * startingTangent;
startingBinormal = startingBinormal - orthogonal;
startingBinormal.Normalize();
}
}
targetEndPoint = ReconstructFromAngles();
ComputeFrenetFrames();
ComputeBishopFrames();
}