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    }