public void Growth()
{
Debug.WriteLine("DiffLine: Growth() Method Called");
//List<Node> tempNodes = new List<Node>(nodes);
for (int i = 0; i < nodes.Count - 1; i++) //Iterate all points
{
DifferentialNode n1 = nodes[i];
DifferentialNode n2 = nodes[i + 1];
double d = n1.position.DistanceTo(n2.position); //Get distance
//Basic growth rule
if (d > maxEdgeLength)
{
int index = nodes.IndexOf(n2);
Point3d middleNodePosition = (n1.position + n2.position) / 2;
nodes.Insert(index,
new DifferentialNode(middleNodePosition.X,
middleNodePosition.Y,
maxForce,
maxSpeed,
this)
);
}
}
//nodes = tempNodes;
}
public Vector3d GetSeparationForce(DifferentialNode n1, DifferentialNode n2)
{
//Debug.WriteLine("DiffLine: GetSeparationForce() Method Called");
Vector3d steer = new Vector3d(0, 0, 0);
double sq_d = Math.Pow(n1.position.X - n2.position.X, 2)
+ Math.Pow(n1.position.Y - n2.position.Y, 2);
if (sq_d > 0 && sq_d < sq_desiredSeparation)
{
Vector3d diff = new Vector3d(n1.position - n2.position);
diff.Unitize();
diff /= Math.Sqrt(sq_d);
steer += diff;
}
return(steer);
}
public void AddNodeAt(DifferentialNode n, int index)
{
//Debug.WriteLine("DiffLine: AddNodeAt({0}) Method Called", index);
nodes.Insert(index, n);
}
public void AddNode(DifferentialNode n)
{
//Debug.WriteLine("DiffLine: AddNode() Method Called");
nodes.Add(n);
}