void FormTheBridgeShape()
{
startVerticesCoordinates = ((GameObject)vertexStart).GetComponent<VertexController>().GetTriodVerticesArray(triodStart);//local coordinates of triods vertices
endVerticesCoordinates = ((GameObject)vertexEnd).GetComponent<VertexController>().GetTriodVerticesArray(triodEnd);
startShape = ((GameObject)vertexStart).GetComponent<VertexController>().GetTriodShape(triodStart);//in the local coordinates of the vertex
endShape = ((GameObject)vertexEnd).GetComponent<VertexController>().GetTriodShape(triodEnd);
ShapeClass originalEndShape = ((GameObject)vertexEnd).GetComponent<VertexController>().GetTriodShape(triodEnd);
UpdateLineRendererPositions();
//------------------------------
//------------------------------
//generate shapes fo the bridge
ShapeClass profile = new ShapeClass();
profile.CopyFrom(startShape);
bridgeShapes = new List<ShapeClass>();
startQ = localStartTriodTransfom.rotation;
endQ = localEndTriodTransform.rotation;
Quaternion originalEndQ = endQ;
Matrix4x4 eM = Matrix4x4.TRS(new Vector3(), endQ * GetTwistQuaternion(), new Vector3(-1, -1, 1));
endQ = QuaternionFromMatrix(eM);
endQ = MultiplyByScalar(endQ, 1f / GetQuaternionLength(endQ));
if(vertexStart == vertexEnd && !isNew)
{
middlePointDistance = middlePointDistanceSelfe;
}
isNew = true;
startMatrix = Matrix4x4.TRS(localStartTriodTransfom.position, startQ, new Vector3(1, 1, 1));
endMatrix = Matrix4x4.TRS(localEndTriodTransform.position, endQ, new Vector3(1, 1, 1));
Matrix4x4 orignalEndMatrix = Matrix4x4.TRS(localEndTriodTransform.position, originalEndQ * GetTwistQuaternion(), new Vector3(1, 1, 1));
originalEndShape.MapsTo(orignalEndMatrix);
startShape.MapsTo(startMatrix);
endShape.MapsTo(endMatrix);
bridgeShapes.Add(startShape);
Quaternion currentRotation = startQ;
List<ShapeClass> firstPassShapes = new List<ShapeClass>();
firstPassShapes.Add(startShape);
for (int i = 1; i <= segmentsCount; i++)
{
Quaternion middleQ = currentRotation;
Matrix4x4 middleMatrix = Matrix4x4.TRS(knotPositions[i], middleQ, new Vector3(1, 1, 1));
Quaternion delta;
if (i < segmentsCount)
{
delta = Quaternion.FromToRotation(middleMatrix.GetColumn(1), (knotPositions[i + 1] - knotPositions[i - 1]).normalized);
}
else
{
delta = Quaternion.FromToRotation(middleMatrix.GetColumn(1), (knotPositions[i] - knotPositions[i - 1]).normalized);
}
middleQ = delta * middleQ;
middleMatrix = Matrix4x4.TRS(knotPositions[i], middleQ, new Vector3(1, 1, 1));
Quaternion twistRotation = Quaternion.AngleAxis(GetTwistAngle() / (float)segmentsCount, middleMatrix.GetColumn(1));
middleQ = twistRotation * middleQ;
middleMatrix = Matrix4x4.TRS(knotPositions[i], middleQ, new Vector3(1, 1, 1));
ShapeClass middleShape = new ShapeClass();
middleShape.CopyFrom(profile);
middleShape.MapsTo(middleMatrix);
firstPassShapes.Add(middleShape);
currentRotation = middleQ;
}
//find angle between end shape and target shape
float errorAngle = ShapeClass.GetAngleBetween(firstPassShapes[firstPassShapes.Count - 1], originalEndShape);
//print(errorAngle);
//bridgeShapesTemp = firstPassShapes;
currentRotation = startQ;
//form the bridge shapes
for (int i = 1; i <= segmentsCount; i++ )
{
Quaternion middleQ = currentRotation;
Matrix4x4 middleMatrix = Matrix4x4.TRS(knotPositions[i], middleQ, new Vector3(1, 1, 1));
Quaternion delta;
if(i < segmentsCount)
{
delta = Quaternion.FromToRotation(middleMatrix.GetColumn(1), (knotPositions[i + 1] - knotPositions[i - 1]).normalized);
}
else
{
delta = Quaternion.FromToRotation(middleMatrix.GetColumn(1), (knotPositions[i] - knotPositions[i - 1]).normalized);
}
middleQ = delta * middleQ;
middleMatrix = Matrix4x4.TRS(knotPositions[i], middleQ, new Vector3(1, 1, 1));
//Quaternion twistRotation = Quaternion.AngleAxis((GetTwistAngle() / (float)segmentsCount) + (errorAngle / (float)segmentsCount), middleMatrix.GetColumn(1));
Quaternion twistRotation = Quaternion.AngleAxis(AnglePiClamp(GetTwistAngle() + errorAngle) / (float)segmentsCount, middleMatrix.GetColumn(1));
middleQ = twistRotation * middleQ;
middleMatrix = Matrix4x4.TRS(knotPositions[i], middleQ, new Vector3(1, 1, 1));
ShapeClass middleShape = new ShapeClass();
middleShape.CopyFrom(profile);
middleShape.MapsTo(middleMatrix);
if(i < segmentsCount)
{
bridgeShapes.Add(middleShape);
}
else
{
ShapeClass tsh = new ShapeClass();
tsh.CopyFrom(middleShape);
tsh.MatchTo(originalEndShape);
//and match to vertices of the Vertex
tsh.WeldToMesh(endVerticesCoordinates, localEndTriodTransform);
bridgeShapes.Add(tsh);
}
currentRotation = middleQ;
}
}
// Use this for initialization
void Start()
{
knotPositions = new Vector3[pointsNumber];
shapes = new List<ShapeClass>();
for (int i = 0; i < pointsNumber; i++ )
{
knotPositions[i] = GetPointPositionInSpline(new Vector3(), new Vector3(0, 1, 0), new Vector3(2, 3, 0), new Vector3(3, 3, 0), (float)i / (float)(pointsNumber - 1));
}
originalShape = new ShapeClass(new Vector3[]{new Vector3(-1, 0, -1), new Vector3(-1, 0, 1), new Vector3(1, 0, 1), new Vector3(1, 0, -1)});
shapes.Add(originalShape);
//startQ = new Quaternion(0, 0, 1, 0);
startQ = Quaternion.Euler(0, 0, 0);
startMatrix = Matrix4x4.TRS(new Vector3(), startQ, new Vector3(1, 1, 1));
endQ = Quaternion.Euler(0, 0, -90);
//endQ = new Quaternion(0, 1, 0, 0);
endMatrix = Matrix4x4.TRS(new Vector3(3, 3, 0), endQ, new Vector3(1, 1, 1));
//endQ = QuaternionFromMatrix(endMatrix);
print("end Quaternion=" + endQ.ToString() + " start Quaternion=" + startQ.ToString());
print("startMatrix=" + startMatrix.ToString());
//calculate angle between quaternions
// float angle = (180 * Mathf.Acos(Quaternion.Dot(startQ, endQ))) / Mathf.PI;
float angle = Mathf.Acos(Quaternion.Dot(startQ, endQ) / (GetQuaternionLength(startQ) * GetQuaternionLength(endQ)));
//print("Angle=" + angle.ToString());
endShape = new ShapeClass();
endShape.CopyFrom(originalShape);
endShape.MapsTo(endMatrix);
//calculate other shapes
for (int i = 1; i < pointsNumber-1; i++ )
{
//Quaternion middleQ = Quaternion.Lerp(startQ, endQ, (float)i * 100 / (float)(pointsNumber - 1));
Quaternion middleQ = GetInterpolatedQuaternion(startQ, endQ, angle, (float)i / (float)(pointsNumber - 1));
print(middleQ.ToString() + " " + ((float)i / (float)(pointsNumber - 1)).ToString());
Matrix4x4 middleMatrix = Matrix4x4.TRS(knotPositions[i], middleQ, new Vector3(1, 1, 1));
ShapeClass middleShape = new ShapeClass();
middleShape.CopyFrom(originalShape);
middleShape.MapsTo(middleMatrix);
shapes.Add(middleShape);
}
shapes.Add(endShape);
}