MOVEMENT Align package movement; import utils.Vec2f; import utils.core; public class Align{ Kinematic character; Kinematic tagert; private float maxRotation; private float tagertRadius; private float slowRadius; private float maxAngularAcceleration; private final float timeToTagert = 0.1f; public Align() {} public Align(Kinematic character, Kinematic tagert, float maxRotation, float tagertRadius, float slowRadius, float maxAngularAcceleration) { this.character = character; this.tagert = tagert; this.maxRotation = maxRotation; this.tagertRadius = tagertRadius; this.slowRadius = slowRadius; this.maxAngularAcceleration = maxAngularAcceleration; } public Kinematic getCharacter() { return character; } public void setCharacter(Kinematic character) { this.character = character; } public Kinematic getTagert() { return tagert; } public void setTagert(Kinematic tagert) { this.tagert = tagert; } public float getMaxAngularAcceleration() { return maxAngularAcceleration; } public void setMaxAngularAcceleration(float maxAngularAcceleration) { this.maxAngularAcceleration = maxAngularAcceleration; } public float getMaxRotation() { return maxRotation; } public void setMaxRotation(float maxRotation) { this.maxRotation = maxRotation; } public float getTagertRadius() { return tagertRadius; } public void setTagertRadius(float tagertRadius) { this.tagertRadius = tagertRadius; } public float getSlowRadius() { return slowRadius; } public void setSlowRadius(float slowRadius) { this.slowRadius = slowRadius; } public SteeringOutput getSteering() { SteeringOutput steering = new SteeringOutput(); float tagertRotation = 0.0f; float rotation = tagert.getOrientation() - character.getOrientation(); rotation = core.mapToRange(rotation); float rotationSize = Math.abs(rotation); if (rotationSize < tagertRadius){ return null; } if (rotationSize > slowRadius){ tagertRotation = maxRotation; }else{//Otherwise calculate a scaled rotion tagertRotation *= rotation/slowRadius; } tagertRotation *= rotation/rotationSize; steering.setAngualar(tagertRotation - character.getRotation()); steering.setAngualar(steering.getAngualar()/timeToTagert); float angularAcceleration = Math.abs(steering.getAngualar()); if (angularAcceleration > maxAngularAcceleration){ steering.setAngualar(steering.getAngualar()/angularAcceleration); steering.setAngualar(steering.getAngualar()*maxAngularAcceleration); } steering.setLinear(new Vec2f(0.0f, 0.0f)); return steering; } } Arrive package movement; import utils.Vec2f; public class Arrive extends Movement{ private float maxSpeed; private float maxRadius; private float slowRadius; private final float timeToTagert = 0.1f; private float tagertSpeed = 0.0f; public Arrive() {} public Arrive(float maxSpeed, float maxRadius, float slowRadius, Kinematic character, Kinematic tagert, float maxAcceleration) { super(character, tagert, maxAcceleration); this.maxSpeed = maxSpeed; this.maxRadius = maxRadius; this.slowRadius = slowRadius; } public float getMaxSpeed() { return maxSpeed; } public void setMaxSpeed(float maxSpeed) { this.maxSpeed = maxSpeed; } public float getMaxRadius() { return maxRadius; } public void setMaxRadius(float maxRadius) { this.maxRadius = maxRadius; } public float getSlowRadius() { return slowRadius; } public void setSlowRadius(float slowRadius) { this.slowRadius = slowRadius; } @Override public SteeringOutput getSteering() { SteeringOutput steering = new SteeringOutput(); Vec2f direction = tagert.getPosition().sub(character.getPosition()); float distance = (float) direction.length(); if (distance > slowRadius){ tagertSpeed = maxSpeed; } else{ tagertSpeed = maxSpeed*distance/slowRadius; } Vec2f tagertVelocity = direction; tagertVelocity.normalize(); tagertVelocity = tagertVelocity.multiconst(tagertSpeed); steering.setLinear(tagertVelocity.sub(character.getVelocity())); if (steering.getLinear().length() > maxAcceleration){ steering.getLinear().normalize(); steering.setLinear(steering.getLinear().multiconst(maxAcceleration)); } steering.setAngualar(0); return steering; } } Kinematic package movement; import utils.Vec2f; public class Kinematic { private Vec2f Position; private float orientation; private Vec2f Velocity; private float rotation; public Kinematic(){ } public Kinematic(Vec2f position,float orientation,Vec2f velocity,float rotation){ this.Position = position; this.orientation = orientation; this.Velocity = velocity; this.rotation = rotation; } /** * @return the position */ public Vec2f getPosition() { return Position; } /** * @param position the position to set */ public void setPosition(Vec2f position) { this.Position = position; } /** * @return the orientation */ public float getOrientation() { return orientation; } /** * @param orientation the orientation to set */ public void setOrientation(float orientation) { this.orientation = orientation; } /** * @return the velocity */ public Vec2f getVelocity() { return Velocity; } /** * @param velocity the velocity to set */ public void setVelocity(Vec2f velocity) { this.Velocity = velocity; } /** * @return the rotation */ public float getRotation() { return rotation; } /** * @param rotation the rotation to set */ public void setRotation(float rotation) { this.rotation = rotation; } /** * * @param steering * @param time */ void update(SteeringOutput steering, float time){ //update the position and orientation this.Position = this.Position.add(this.Velocity.multiconst(time).add(steering.getLinear().multiconst(time* time*0.5f))); this.orientation = this.rotation + this.rotation*time + steering.getAngualar() + time*time*0.5f; this.Velocity = this.Velocity.add(steering.getLinear().multiconst(time)); this.rotation = this.rotation + steering.getAngualar()*time; } /** * * @param kinematicOutput * @param time */ void update(KinematicOutput kinematicOutput, float time){ this.Position = this.Position.add(this.Velocity.multiconst(time)); this.orientation = this.orientation + this.rotation*time; this.Velocity = kinematicOutput.getVelocity(); this.rotation = kinematicOutput.getRotation(); } void update(SteeringOutput steering, float maxSpeed, float time){ //Update the position and orientation this.Position = this.Position.add(this.getVelocity().multiconst(time)); this.orientation *= this.rotation*time; //and the velocity and rotation this.Velocity = this.Velocity.add(steering.getLinear().multiconst(time)); this.orientation += steering.getAngualar()*time; //check for speeding and clip if (this.Velocity.length() > maxSpeed){ this.Velocity.normalize(); this.Velocity = this.getVelocity().multiconst(maxSpeed); } } /** * * @param currentOrientation * @param velocity * @return */ public float getNewOriention(float currentOrientation, Vec2f velocity){ if (velocity.length() > 0){ return (float) Math.atan2(-velocity.getX(), velocity.getY()); } return currentOrientation; } public Vec2f asVector(){ return new Vec2f((float)Math.sin(getOrientation()), (float)Math.cos(getOrientation())); } /** * * @return */ @Override public String toString() { return "Velocity = " + Velocity + ", orientation = " + orientation + ", Velocity = " + Velocity + ", rotation = " + rotation; //To change body of generated methods, choose Tools | Templates } } KinematicMovement package movement; public abstract class KinematicMovement { Kinematic character; Kinematic tagert; float maxSpeed; public KinematicMovement() { } public KinematicMovement(Kinematic character, Kinematic tagert, float maxSpeed) { this.character = character; this.tagert = tagert; this.maxSpeed = maxSpeed; } public Kinematic getCharacter() { return character; } public void setCharacter(Kinematic character) { this.character = character; } public Kinematic getTagert() { return tagert; } public void setTagert(Kinematic tagert) { this.tagert = tagert; } public float getMaxSpeed() { return maxSpeed; } public void setMaxSpeed(float maxSpeed) { this.maxSpeed = maxSpeed; } abstract public KinematicOutput getSteering(); } KinematicOutput package movement; import utils.Vec2f; public class KinematicOutput { private Vec2f velocity; private float rotation; public KinematicOutput() {} public KinematicOutput(Vec2f velocity, float rotation) { this.velocity = velocity; this.rotation = rotation; } public Vec2f getVelocity() { return velocity; } public void setVelocity(Vec2f velocity) { this.velocity = velocity; } public float getRotation() { return rotation; } public void setRotation(float rotation) { this.rotation = rotation; } @Override public String toString() { return "Velocity = " + velocity + ", rotation = " + rotation; //To change body of generated methods, choose Tools | Templates } } KinematicSeek package movement; public class KinematicSeek extends KinematicMovement{ /** * * @return */ @Override public KinematicOutput getSteering(){ KinematicOutput steering = new KinematicOutput(); //Seek steering.setVelocity(tagert.getPosition().sub(character.getPosition())); steering.getVelocity().normalize(); steering.setVelocity(steering.getVelocity().multiconst(maxSpeed)); character.setOrientation(character.getNewOriention(character.getOrientation(), steering.getVelocity())); steering.setRotation(0); return steering; } } KinematicWander package movement; import utils.core; public class KinematicWander extends KinematicMovement{ private float maxRotation; public KinematicWander() { } public float getMaxRotation() { return maxRotation; } public void setMaxRotation(float maxRotation) { this.maxRotation = maxRotation; } @Override public KinematicOutput getSteering() { KinematicOutput steering = new KinematicOutput(); steering.setVelocity(character.asVector().multiconst(maxSpeed)); steering.setRotation(core.randomBinomial()*maxRotation); return steering; } } Movement package movement; public abstract class Movement { Kinematic character; Kinematic tagert; float maxAcceleration; public Movement() { } public Movement(Kinematic character, Kinematic tagert, float maxAcceleration) { this.character = character; this.tagert = tagert; this.maxAcceleration = maxAcceleration; } public Kinematic getCharacter() { return character; } public void setCharacter(Kinematic character) { this.character = character; } public Kinematic getTagert() { return tagert; } public void setTagert(Kinematic tagert) { this.tagert = tagert; } public float getMaxAcceleration() { return maxAcceleration; } public void setMaxAcceleration(float maxAcceleration) { this.maxAcceleration = maxAcceleration; } public abstract SteeringOutput getSteering(); } Seek package movement; public class Seek{ public Kinematic tagert; public Kinematic character; public float maxAcceleration; public Seek() { } public Seek(Kinematic tagert, Kinematic character, float maxAcceleration) { this.tagert = tagert; this.character = character; this.maxAcceleration = maxAcceleration; } public Kinematic getTagert() { return tagert; } public void setTagert(Kinematic tagert) { this.tagert = tagert; } public Kinematic getCharacter() { return character; } public void setCharacter(Kinematic character) { this.character = character; } public float getMaxAcceleration() { return maxAcceleration; } public void setMaxAcceleration(float maxAcceleration) { this.maxAcceleration = maxAcceleration; } public SteeringOutput getSteering() { SteeringOutput steering = new SteeringOutput(); steering.setLinear(this.tagert.getPosition().sub(this.character.getPosition())); steering.getLinear().normalize(); steering.setLinear(steering.getLinear().multiconst(maxAcceleration)); steering.setAngualar(0); return steering; } @Override public String toString() { return "Linear = " + linear + ", Angula = " + angualar; //To change body of generated methods, choose Tools | Templates } } PATHFINDING AStar package pathfinding; import java.util.LinkedList; import java.util.Set; import utils.core; public class AStar implements Graph{ private static int nodesProcessed = 0; public LinkedList pathfindAStar(Graph graph, Node start, Node end, Heuristic heuristic){ NodeRecord startRecord = new NodeRecord(); startRecord.setNode(start); startRecord.setConnection(null); startRecord.setCostSoFar(0); NodeRecord endNodeRecord; NodeRecord curent = new NodeRecord(); LinkedList path = new LinkedList(); heuristic = new Heuristic(end); float endNodeHeuristic = 0.0f; startRecord.setEstimoteTotalCost(heuristic.estimote(start)); PathfindingList open = new PathfindingList(); open.add(startRecord); PathfindingList closed = new PathfindingList(); while (open.list.size() > 0) { curent = open.smallestElement(); if (curent.getNode().equals(end)){ break; } LinkedList connections = graph.getConnection(curent.getNode()); for (Connection connection : connections){ nodesProcessed++; Node endNode = connection.getEnd(); float endNodeCost = (curent.getCostSoFar() + connection.getCost()); if (closed.contains(endNode)){ endNodeRecord = closed.find(endNode); if (endNodeRecord.getCostSoFar() 0){ return (float) Math.atan2(-velocity.getX(), velocity.getY()); } return currentOrientation; } public static float getWhereYoureGoing(Vec2f velocity){ if (velocity != null){ return (float) Math.atan2(-velocity.getX(), velocity.getY()); } return 0; } public static float min(float a, float b){ return a < b ? a : b; } public static float max(float a, float b){ return a > b ? a : b; } public static double dot(Vec2f a, Vec2f b) { return (a.getX()*b.getX()) + a.getY()*b.getY(); } public static LinkedList reverse(LinkedList path){ Stack stack = new Stack(); LinkedList soln = new LinkedList(); for (Connection connection : path){ stack.push(connection); } while (stack.size() > 0) { soln.add(stack.pop()); } return soln; } } Property package utils; public class Property implements Cloneable{ public Object getValue(){ return null; } public void setAttribute(String val, String type){ } public Property[] createArray(int size){ return new Property[size]; } @Override protected Object clone() throws CloneNotSupportedException { return new Property(); } public String vrmlType(){ return null; } } Vec2f package utils; import java.util.StringTokenizer; public class Vec2f extends Property{ private float x; private float y; public Vec2f(){} public Vec2f(float x, float y){ this.x = x; this.y = y;} public Vec2f(Vec2f v){ x = v.x; y = v.y;} public static String vrmlType_S(){ return "Vec2f";} @Override public String vrmlType() { return "Vec2f";} @Override protected Object clone() throws CloneNotSupportedException { return new Vec2f(this);} @Override public Property[] createArray(int size) { return new Vec2f[size];} @Override public void setAttribute(String val, String type) { try { int v = 0; StringTokenizer st = new StringTokenizer(val, "() \\t\\n\\r\\f"); while (st.hasMoreElements()) { try { switch(v){ case 0:setX(Float.parseFloat(st.nextToken()));break; case 1:setY(Float.parseFloat(st.nextToken()));break; default: st.nextToken();break; } v++; } catch (Exception e) { e.printStackTrace(); }}} catch (Exception e) { System.out.println("Vec2f.setAttribute("+val+","+type+") " + e); } } public Vec2f min(Vec2f a, Vec2f b){ return a.length() < b.length() ? a : b; } public Vec2f max(Vec2f a, Vec2f b){ return a.length() > b.length() ? a : b; } public Vec2f sub(Vec2f other){ this.setX(getX() - other.getX()); this.setY(getY() - other.getY()); return this; } public Vec2f add(Vec2f other){ this.setX(getX() + other.getX()); this.setY(getY() + other.getY()); return this; } public Vec2f add(float ct){ this.setX(this.getX() + ct); this.setY(this.getY() + ct); return this; } public Vec2f multi(Vec2f other){ this.setX(this.getX()*other.getX()); this.setY(this.getY()*other.getY()); return this; } public Vec2f sub(Vec2f other_vec1, Vec2f other_vec2){ Vec2f vec2f = new Vec2f(); vec2f.setX(other_vec1.getX() - other_vec2.getX()); vec2f.setY(other_vec1.getY() - other_vec2.getY()); return vec2f; } public Vec2f add(Vec2f other_vec1, Vec2f other_vec2){ Vec2f vec2f = new Vec2f(); vec2f.setX(other_vec1.getX() + other_vec2.getX()); vec2f.setY(other_vec1.getY() + other_vec2.getY()); return vec2f; } public Vec2f multiconst(Vec2f vector, double ct){ Vec2f vec2f = new Vec2f(); vec2f.setX((float) (vector.getX()*ct)); vec2f.setY((float) (vector.getY()*ct)); return vec2f; } public Vec2f multiconst(double ct){ this.setX((float) (getX()*ct)); this.setY((float) (getY()*ct)); return this; } public Vec2f divconst(double ct){ Vec2f vec2f = new Vec2f(); vec2f.multiconst(1/ct); return this; } public float getX() { return x; } public void setX(float x) { this.x = x; } public float getY() { return y; } public void setY(float y) { this.y = y; } public double length() { return Math.sqrt(getX()*getX() + getY()*getY()); } public void normalize(){ float l = (float) Math.sqrt(getX()*getX() + getY()*getY()); if (l > 0){ this.setX((float) (this.getX()*1.0/l)); this.setY((float) (this.getY()*1.0/l)); }}}