001 package org.server.motion; 002 003 import java.util.*; 004 import java.io.*; 005 import java.net.*; 006 import java.awt.*; 007 import java.awt.event.*; 008 import javax.swing.*; 009 import javax.swing.event.*; 010 import java.awt.geom.*; 011 import org.realobject.*; 012 013 public class SimpleMotionSimulator implements MotionSimulator { 014 015 private HashMap<String, Point2D.Double> flag_location_map_; 016 private ArrayList<RCRSVirtualSoccerEcoBe> ecobe_list_; 017 private Ball ball_; 018 019 public void init(HashMap<String, Point2D.Double> flag_location_map, ArrayList<RCRSVirtualSoccerEcoBe> ecobe_list, Ball ball) { 020 flag_location_map_ = flag_location_map; 021 ecobe_list_ = ecobe_list; 022 ball_ = ball; 023 } 024 025 public void simulateStep(double step_time) { 026 ball_.setForce(0, 0); 027 Point2D.Double v = ball_.getVelocity(); 028 ball_.setVelocity(v.getX()*0.9, v.getY()*0.9); 029 030 for(int i=0; i<ecobe_list_.size(); i++) { 031 RCRSVirtualSoccerEcoBe target = ecobe_list_.get(i); 032 String command_type = target.getCommandType(); 033 double[] command_args = target.getCommandArgs(); 034 if(command_type==null) continue; 035 if(command_type.equals("kick")) { 036 double distance = ball_.getLocation().distance(target.getLocation()); 037 if(distance <= (ball_.getRadius()+target.getRadius())) { 038 double ecobe_angle = target.getAngle(); 039 double angle = command_args[0]*Math.PI/180.0+ecobe_angle; 040 double force = 10*command_args[1]; 041 ball_.addForce(force*Math.cos(angle), force*Math.sin(angle)); 042 } 043 }else if(command_type.equals("wheel_velocities")) { 044 double left = command_args[0]; 045 double right = command_args[1]; 046 double drive = 0.1*(left+right)/2; 047 double rotate = (right-left)/2; 048 double angle = target.getAngle(); 049 target.move(drive*Math.cos(angle), drive*Math.sin(angle)); 050 target.rotate(rotate); 051 } 052 } 053 Point2D.Double f = ball_.getForce(); 054 Point2D.Double a = new Point2D.Double(f.getX()/ball_.getMass(), f.getY()/ball_.getMass()); 055 v = ball_.getVelocity(); 056 ball_.setVelocity(v.getX()+a.getX()*step_time, v.getY()+a.getY()*step_time); 057 ball_.move(v.getX()*step_time, v.getY()*step_time); 058 } 059 }