final fix instersection and GK

Author: mahi97

src/my_strategy.rs

@@ -46,7 +46,7 @@ impl Strategy for MyStrategy {
     	// Choose My Role 1. GK, 2. DEF, 3. OFF 4. SUP
         let my_role = self.coach.find_role(me, _game, _rules);
     	// Execute My Role
-        let oppGoal = Vec2::new(0.0, -self.rules.arena.depth/2.0);
+        let oppGoal = Vec2::new(0.0, self.rules.arena.depth/2.0 + 20.0);
 
         match my_role {
             Role::NONE =>  println!("No Role is Selected"),
@@ -69,7 +69,9 @@ fn get_bisect(seg: &Seg2, vec: &Vec2) -> Seg2{
     let v2 = seg.origin().rotateVector(-ang.deg());
     let s = Seg2::new(*vec, v2);
     let v3 = s.intersection(*seg);
-    println!("AAA {:?} {:?}",ang, v3);
+    if ! v3.is_valid() {
+        return Seg2::new(*vec, (seg.origin() + seg.terminal()) * 0.5)
+    }
     Seg2::new(*vec, v3)
 }
 
@@ -78,15 +80,14 @@ impl MyStrategy {
 
 
     fn gk(&mut self) {
-        let ball_pos = self.game.ball.position();
+        let ball_pos = self.game.ball.position() + self.game.ball.velocity() * 0.1;
         let goal_line = Seg2{
             origin:   Vec2{x: self.rules.arena.goal_width/2.0, y:-self.rules.arena.depth/2.0},
             terminal: Vec2{x:-self.rules.arena.goal_width/2.0, y:-self.rules.arena.depth/2.0}
         };
         let ball_seg = Seg2::new(self.game.ball.position(), self.game.ball.velocity()*100.0);
         let biset = get_bisect(&goal_line, &ball_pos);
         let mut target = biset.terminal();
-        println!("DFAS");
         if self.game.ball.velocity().y < -1.0 { // KICK
             target = goal_line.intersection(ball_seg);
             if !target.is_valid() {
@@ -100,16 +101,11 @@ impl MyStrategy {
         } else if target.x > self.rules.arena.goal_width/2.0 - 1.5{
             target.x = self.rules.arena.goal_width/2.0 - 1.5;
         }
-        self.gtp(&target);
-        // let my = self.me.position();
-        // if self.game.ball.position().dist(Vec2::new(0.0, -self.rules.arena.depth/2.0)) < 20.0 && self.game.ball.velocity().len() < 2.0 {
-        //     self.kick(&((ball_pos - my) * 10.0));
-        // }
 
+        self.gtp(&target);
+        self.action.jump_speed = 0.0;
         if ball_pos.dist(self.me.position()) < 3.0 && self.game.ball.height() > 2.5 {
             self.action.jump_speed = self.rules.ROBOT_MAX_JUMP_SPEED;
-        } else {
-            self.action.jump_speed = 0.0;
         }
 
     }

src/seg2.rs

@@ -48,7 +48,31 @@ impl Seg2 {
         Line2::new2(self.origin, self.terminal)
     }
 
+    fn contains(&self, p: &Vec2) -> bool{
+        (p.x - self.origin.x) * (p.x - self.terminal.x) <= EPSILON
+        && (p.y - self.origin.y) * (p.y - self.terminal.y) <= EPSILON
+    }
+
+    fn nearest_point(&self, p: &Vec2) -> Vec2{
+        let v = self.terminal - self.origin;
+        let len_square = v.len() * v.len();
+        if len_square - 0.0 < EPSILON {
+            return self.origin;
+        }
+        let inner_product = v.inner_product( &(*p - self.origin) );
+        if inner_product <= 0.0 {
+            return self.origin;
+        } else if inner_product >= len_square {
+            return self.terminal;
+        }
+        self.origin + v * (inner_product / len_square)
+    }
+
     fn intersection(&self, other : Seg2) -> Vec2 {
-        self.line().intersection(other.line())
+        let sol = self.line().intersection(other.line());
+        if ! sol.is_valid() || ! self.contains(&sol) || ! other.contains(&sol) {
+            return VEC2INVALID;
+        }
+        sol
     }
 }

src/vec2.rs

@@ -63,6 +63,9 @@ impl Vec2 {
         v
     }
 
+    fn inner_product(&self, p: &Vec2) -> f64 {
+        self.x * p.x + self.y * p.y
+    }
 }
 
 // Subtraction operation for vectors