public static GridObstacleState FromObstacleState(ObstacleState obst, GridCarModelState state) { double d = ComMath.Normal(Math.Sqrt(obst.pp.position.X * obst.pp.position.X + obst.pp.position.Y * obst.pp.position.Y), GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, 1); double a = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, 1); double ang = Math.PI - (Math.Atan2(obst.pp.position.Y, obst.pp.position.X) + Math.PI) + state.TargetAngle - state.TargetFinishAngle; if (ang > Math.PI) ang -= 2 * Math.PI; if (ang < -Math.PI) ang += 2 * Math.PI; double AA = -2 * d * Math.Cos(ang); double BB = d * d; double obstdist = Math.Sqrt(a * a + BB + AA * a); double obstang = state.TargetAngle + Math.Sign(ang) * Math.Acos((a * a + obstdist * obstdist - d * d) / (2 * a * obstdist)); GridObstacleState gos = new GridObstacleState(obstdist, obstang, obst.radius); return gos; }
public static GridObstacleState FromObstacleState(ObstacleState obst, GridCarModelState state) { double d = ComMath.Normal(Math.Sqrt(obst.pp.position.X * obst.pp.position.X + obst.pp.position.Y * obst.pp.position.Y), GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, 1); double a = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, 1); double ang = Math.PI - (Math.Atan2(obst.pp.position.Y, obst.pp.position.X) + Math.PI) + state.TargetAngle - state.TargetFinishAngle; if (ang > Math.PI) { ang -= 2 * Math.PI; } if (ang < -Math.PI) { ang += 2 * Math.PI; } double AA = -2 * d * Math.Cos(ang); double BB = d * d; double obstdist = Math.Sqrt(a * a + BB + AA * a); double obstang = state.TargetAngle + Math.Sign(ang) * Math.Acos((a * a + obstdist * obstdist - d * d) / (2 * a * obstdist)); GridObstacleState gos = new GridObstacleState(obstdist, obstang, obst.radius); return(gos); }
internal void AddObstacle(GridObstacleState gos) { throw new NotImplementedException(); }