3D Path Optimization Problem

Hello All,

I'm trying to optimize a set of way points for a path through a tunnel. This tunnel is described as a solid body from an stl file, and I am currently using the vertices of the stl mesh file as my boundary for my way points.

I already have a vector of 3D coordinates that describe a line path through this tunnel. What I now want to do is move these coordinates so that I can minimize the bend angle between each adjacent point in the vector. THe start and end points cannot move, and all points must lie at least a known distance away from the tunnel walls.

I have made a function that returns a true or false depending on whether a point is or is not inside and beyond a known distance from the walls of the tunnel. I also have a function that can tell me what the bend angle between each adjacent point is.

My question is: is the MATLAB optimization toolbox somehting I can use? What I'm stuck at is how to give the optimizer constraints if my function outputs true false. Also, I also want to incorporate a constraint that prevents any way point from being too close to another (say I want the waypoints to be evenly spaced apart).

Can anyone help me with this? I've been looking at fmincon but I am still puzzled.

Thank you in advance, and I hope someone can shed some light on this problem!

llam