/*
* Copyright (c) 2011 adVolition
*
* Permission is hereby granted, free of charge, to any person
* obtaining a clone of this software and associated documentation
* files (the "Software"), to deal in the Software without
* restriction, including without limitation the rights to use,
* clone, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following
* conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* Except as contained in this notice, the name(s) of the above
* copyright holders shall not be used in advertising or otherwise to
* promote the sale, use or other dealings in this Software without prior
* written authorization.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
* OTHER DEALINGS IN THE SOFTWARE.
*/
// NOTE: Can't get sonar distance to convert correctly for concave and convex barriers. 3/15/11
package ArenaSimulator;
import NeuralNetwork.*;
import java.awt.Graphics2D;
import java.awt.Color;
import userinterface.Trace;
import com.thoughtworks.xstream.annotations.*;
/**
* Defines the physical properties of a robot operating in an arena.
*/
@XStreamAlias("robot") public class Robot {
@XStreamAlias("score") @XStreamAsAttribute private Double _score;
@XStreamAlias("brain") private Brain _brain;
@XStreamAlias("mutability") private Mutability _fidelity;
private transient Clan _clan;
private transient Vector2D _currentPosition;
private transient Vector2D _futurePosition;
private transient Double _currentFacing; // Robot facing in degrees.
private transient Double _futureFacing;
private transient Vector2D _robotsXAxis;
private transient Vector2D _robotsYAxis;
private transient Vector2D _linearVelocityVector;
private transient Vector2D _linearDisplacementVector;
private transient Double _minimumSonarDistance;
private transient Double _timeIncrement;
private transient Double _arenaTime;
private transient Double _stallTime;
private transient Trace _sonarTrace;
private transient Trace _timeTrace;
private transient Boolean _robotIsStuck;
private transient Boolean _robotHasFallen;
public Robot(Clan clan) {
_clan = clan;
configureObject();
}
public Object readResolve() {
configureObject();
return this;
}
/** Used by class constructor and readResolve routines to ensure that newly created
* objects are configured properly.
**/
private void configureObject() {
if (_score == null) _score = -10000.0;
if (_brain == null) _brain = new Brain();
if (_fidelity == null) _fidelity = new Mutability();
_brain.setFidelity(_fidelity);
_stallTime = 0.0;
_linearVelocityVector = new Vector2D();
_linearDisplacementVector = new Vector2D();
_minimumSonarDistance = 0.0;
_currentPosition = new Vector2D();
_futurePosition = new Vector2D();
_currentPosition.cartesianSet(80.0,30.0);
_currentFacing = 0.0;
_futureFacing = 0.0;
_robotsXAxis = new Vector2D();
_robotsYAxis = new Vector2D();
setRobotIsStuck(false);
setRobotHasFallen(false);
_timeIncrement = 1.0;
_arenaTime = 0.0;
_sonarTrace = new Trace();
_timeTrace = new Trace();
_sonarTrace.define("Sonar Distance", 1, "", "cm");
_timeTrace.define("Time", 1, "", "s");
}
public void addScore(Double score) {
_score += score - (double)size();
}
public Robot getClone() {
Robot robot = new Robot(_clan);
robot.setBrain((Brain)_brain.getClone());
robot.resetScore();
return robot;
}
public void setBrain(Brain newBrain) {
_brain = newBrain;
}
/**
* Checks to see if the robot hit the ends of the barrier. If it has hit a barrier.
* it tells the robot's brain that the motors are not moving. This function also
* checks to see if the barrier appears on the robot's sonar and records it's distance if it
* is the closest barrier seen by the robot so far.
* @param feature
* @return
*/
public Boolean encounteredFeature(Feature feature) {
// See if barrier shows up on the robot's sonar.
Double sonarDistance = getSonarDistance(feature);
if (sonarDistance < _minimumSonarDistance) _minimumSonarDistance = sonarDistance;
// See if robot has colided with the barrier.
for (int i = 1; i < feature.getNodeCount(); i ++) {
if (_futurePosition.getDistanceToLine(feature.getNode(i - 1), feature.getNode(i)) <= _clan.getRobotRadius()) {
_futurePosition.vectorSet(_currentPosition);
return true;
}
}
return false;
}
/**
* Sets the current facing of the robot.
* @param newValue (degrees)
*/
public void setCurrentFacing(Double newValue) {
while (newValue < 0.0) {
newValue += 360.0;
}
while (newValue > 360.0) {
newValue -= 360.0;
}
_currentFacing = newValue;
_futureFacing = newValue;
_linearVelocityVector.polarSet(_clan.getRobotLinearVelocity(), _currentFacing);
_robotsYAxis.polarSet(1.0, _currentFacing);
_robotsXAxis.polarSet(1.0, _currentFacing + 90.0);
}
/**
* Sets the current position of the robot. Increments the stall time if the
* robot has not changed its position.
* @param newPosition (centimeters)
*/
private void setCurrentPosition(Vector2D newPosition) {
if (_currentPosition.equals(newPosition)) {
_stallTime += _timeIncrement;
} else {
_currentPosition.vectorSet(newPosition);
_stallTime = 0.0;
}
}
public void setRobotIsStuck(Boolean status) {
_robotIsStuck = status;
}
public void setRobotHasFallen(Boolean status) {
_robotHasFallen = status;
}
/**
* Returns the total number of nodes and edges contained in the robot's brain.
* @return
*/
public int size() {
return _brain.size();
}
/**
* Computes the future state of the robot.
* @param timeIncrement
*/
public void computeFutureState(Double timeIncrement) {
_timeIncrement = timeIncrement;
_brain.setSonarNode(computeSonarNodeValue());
_brain.computeFutureState(timeIncrement);
if (_brain.moveForward()) {computeForwardMotion(timeIncrement);}
if (_brain.moveBackward()) {computeBackwardMotion(timeIncrement);}
if (_brain.turnRight()) {computeRightRotation(timeIncrement);}
if (_brain.turnLeft()) {computeLeftRotation(timeIncrement);}
//computeForwardMotion(timeIncrement);
_minimumSonarDistance = _clan.getRobotRestingSonarDistance();
}
private void computeForwardMotion(Double timeIncrement) {
_linearDisplacementVector.vectorSet(_linearVelocityVector);
_linearDisplacementVector.multiplyBy(timeIncrement);
_futurePosition.addVector(_linearDisplacementVector);
}
private void computeBackwardMotion(Double timeIncrement) {
_linearDisplacementVector.vectorSet(_linearVelocityVector);
_linearDisplacementVector.multiplyBy(timeIncrement);
_futurePosition.subtractVector(_linearDisplacementVector);
}
private void computeRightRotation(Double timeIncrement) {
_futureFacing += timeIncrement * _clan.getRobotAngularVelocity();
}
private void computeLeftRotation(Double timeIncrement) {
_futureFacing -= timeIncrement * _clan.getRobotAngularVelocity();
}
private Double computeSonarNodeValue() {
if (_minimumSonarDistance <= _clan.getRobotRestingSonarDistance()) {
return _minimumSonarDistance / _clan.getRobotRestingSonarDistance() * 0.6;
}
return 0.8;
}
/**
* Returns the average performance score for the robot.
* @return
*/
public Double getScore() {
return _score;
}
/**
* Returns the distance in centimeters from the center of the robot to
* barrier in front of the robot.
* @param feature
* @return
*/
private Double getSonarDistance(Feature feature) {
Vector2D pointA;
Vector2D pointB;
for (int i = 1; i < feature.getNodeCount(); i ++) {
pointA = feature.getNode(i - 1).copy();
pointB = feature.getNode(i).copy();
pointA.transformCoordinates(_currentPosition, _robotsXAxis, _robotsYAxis);
pointB.transformCoordinates(_currentPosition, _robotsXAxis, _robotsYAxis);
if (pointA.getX() * pointB.getX() <= 0.0) { // Confirm the target crosses the y axis.
Double distance = getYIntercept(pointA, pointB) - _clan.getRobotRadius();
if (distance >= 0.0) {
//if (distance < 20.0) _linearVelocityVector.cartesianSet(0.0, 0.0);
if (feature.isWall()) {
return distance;
} else if (feature.isLedge()) {
return 10000000.0; // The sonar is looking over an edge. Distance is essentially infinate.
} //
}
}
}
return _clan.getRobotRestingSonarDistance();
}
/**
* Returns the amount of time that the robot has been stalled.
* @return
*/
public Double getStallTime() {
return _stallTime;
}
/**
* Returns the y intercept of a line defined by pointA and pointB.
* @param pointA
* @param pointB
* @return
*/
private Double getYIntercept(Vector2D pointA, Vector2D pointB) {
//y = y1 + [(y2 - y1) / (x2 - x1)]ยท(x - x1) where x = 0
Double x1 = pointA.getX();
Double y1 = pointA.getY();
Double x2 = pointB.getX();
Double y2 = pointB.getY();
return y1 - x1 * (y2 - y1)/(x2 - x1);
}
public void implementFutureState() {
// Robot only moves forward if it hasn't hit an obsticle.
if (! _robotIsStuck && ! _robotHasFallen) {
setCurrentPosition(_futurePosition);
setCurrentFacing(_futureFacing);
}
_brain.implementFutureState();
_arenaTime += _timeIncrement;
updateTraces();
}
/**
* Returns the radian value of angle expressed in degrees.
* @param degrees
* @return
*/
private double getRadians(Double degrees) {
return degrees / 180.0 * Math.PI;
}
/**
* Redraws the robot.
* @param xOffset
* @param yOffset
* @param scale
* @param g2d
*/
public void draw(int xOffset, int yOffset, Double scale, Graphics2D g2d) {
int diameter = (int) (_clan.getRobotRadius() * 2.0 / scale);
int aX = _currentPosition.getScaledX(scale) + xOffset;
int aY = _currentPosition.getScaledY(scale) + yOffset;
g2d.setColor(Color.black);
g2d.drawArc(aX - diameter / 2, aY - diameter / 2, diameter, diameter, 0, 360);
// Draw the facing indicator.
int bX = aX + (int)(_clan.getRobotRadius() * Math.sin(getRadians(_currentFacing)) / scale);
int bY = aY + (int)(_clan.getRobotRadius() * Math.cos(getRadians(_currentFacing)) / scale);
g2d.drawLine(aX, aY, bX, bY);
// Draw traces.
// _sonarTrace.redraw(g2d);
// _timeTrace.redraw(g2d);
}
/**
* Resets the robot after every run.
*/
public void reset() {
_robotIsStuck = false;
_robotHasFallen = false;
_brain.reset();
_stallTime = 0.0;
}
public void resetScore() {
// Penalize robot for size of brain.
_score = 0.0;
}
/**
* Sets the clan to which the robot belongs.
* @param clan
*/
public void setClan (Clan clan) {
_clan = clan;
}
/**
* Sets the robot's x and y position in centimeters.
* @param x
* @param y
*/
public void setPosition(Double x, Double y) {
_currentPosition.cartesianSet(x, y);
_futurePosition.cartesianSet(x, y);
}
/**
* This function is called to update traces on robot variables.
*/
public void updateTraces() {
_sonarTrace.update(_minimumSonarDistance);
_timeTrace.update(_arenaTime);
}
}