I have a graph of convex sets where I am solving for the optimal path through multiple regions. I am working on improving the costs between each vertex (polygon-shaped regions). The cost to travel along the edge from vertex u to vertex v should be the distance from u to v multiplied by a scaling factor for traveling through that vectex. (The longer the edge travels through the vertex, the more effect that vertex will have on the cost)

i.e. cost_of_edge_from_u_to_v = distance_from_u_to_v * (factor_while_traveling_through_u * distance_from_start_point_to_end_of_u + factor_while_traveling_through_v * distance_from_start_of_v_to_end_point)

I am adding a cost to the edge as a drake::symbolic::expression. I would like to compute the intersection point of the path edge from u to v with one of the outer edges of the polygon, v. Using this intersection point, I will be able to compute the ratio of time in polygon u and in polygon v which will allow me to use the equation mentioned above to get a upper bound for the cost. Currently, I am running into issues of dividing by the determinate which can be zero at some points when solving for the intersection point.

An example of this could be a vehicle traveling from one region to another where the ground of the first region is muddy and the second is made of concrete. While the distance might be the same in region 1 and region 2 that the vehicle has to travel through, the cost will be much higher for traveling through the mud.

If there is a better way to solving this problem, I am open to suggestions