public void ExpandTree(Tree expansionTree, Tree endTree)
{
// Get a free-ish space sample
var newNode = expansionTree.SampleFreeSpace();
int i = 0;
while (newNode == null && i < 100)
{
newNode = expansionTree.SampleFreeSpace();
i++;
}
if (newNode == null)
{
return;
}
// Check for collisions
if (IsCollision(newNode.Point.transform))
{
return;
}
//Debug.Log("First soln is: " + printsoln);
//Debug.Log("Solution list is " + newNode.ParentNode.GetSolutionPath().Count + " steps long");
var movePath = ikSolver.TestPath(newNode.ParentNode.GetSolutionPath(), newNode.ParentNode.Point.Joints, newNode.Point.transform);
if (movePath == null)
{
return;
}
Debug.Log("Found a point");
/* Debug Print Array Start */
string printsoln = "";
foreach (var solnstep in movePath)
{
printsoln += "( ";
foreach (var joint in solnstep)
{
printsoln += joint.ToString() + ", ";
}
printsoln += ")\n";
}
//Debug.Log("Move Path is: " + printsoln);
/* Debug Print Array End */
newNode.AddSolutionSteps(movePath);
newNode.Point.AddJointAngles(ikSolver.GetJointPose(movePath));
/* Debug Print Array Start */
printsoln = "";
foreach (var solnpoint in newNode.Point.Joints)
{
printsoln += solnpoint.ToString() + "\n";
}
//Debug.Log("Joint set is: " + printsoln);
/* Debug Print Array End */
expansionTree.Add(newNode.Clone());
//Debug.Log("Added node: " + cClose.transform);
var soln = SolutionPathList.AddSolutionIfExists(newNode, endTree);
if (soln != null)
{
Debug.Log("Found a solution!");
Debug.Log("Solution: " + soln.ToString());
motionController.StartSolutionRun(soln);
}
}
