public double distance(Units otherUnit) {
return position.distance(otherUnit.position);
}
/** Movement given no collision */
public synchronized void moveNoCollision(double time) {
if (time <= 0) {
throw new InvalidParameterException("Cannot move the unit, time <= 0!");
}
if (Double.isNaN(time)) {
throw new RuntimeException(
"time is NaN. speed is " + speed + " moving angle is " + movingAngle);
}
// Check if the unit is facing the required direction
if (movingAngle == finalAngle) { // Already facing that direction
if (this.destination != null) {
if (speed * time >= position.distance(destination)) {
xVelocity = 0;
yVelocity = 0;
position = destination;
return;
}
}
xVelocity = speed * Math.cos(movingAngle);
yVelocity = speed * Math.sin(movingAngle);
position.setX(Geometry.fixX(position.getX() + xVelocity * time));
position.setY(Geometry.fixY(position.getY() + yVelocity * time));
} else { // Need to turn to that direction
double distance = finalAngle - movingAngle;
double alternative = -(2 * Math.PI - finalAngle + movingAngle);
if (distance < 0) {
while (alternative <= -Math.PI) {
alternative += 2 * Math.PI;
}
} else {
while (alternative >= Math.PI) {
alternative -= 2 * Math.PI;
}
}
double angularDisplacement;
if (Math.abs(distance) < Math.abs(alternative)) {
angularDisplacement = distance;
} else {
angularDisplacement = alternative;
}
if (angularSpeed * time < Math.abs(angularDisplacement)) { // Does not finish turning
movingAngle +=
((angularDisplacement) / Math.abs(angularDisplacement)) * angularSpeed * time;
xVelocity = 0;
yVelocity = 0;
} else { // Finish turning with extra or no time left
movingAngle = finalAngle;
if (Double.isNaN(angularSpeed)) {
throw new RuntimeException(
"Angular speed is NaN. angulardisplacement is " + angularDisplacement);
}
if (Double.isInfinite(angularSpeed)) {
throw new RuntimeException(
"Angular speed is isInfinite. angulardisplacement is " + angularDisplacement);
}
if (Double.isNaN(angularDisplacement)) {
throw new RuntimeException("angularDisplacement is NaN. angularSpeed is " + angularSpeed);
}
if (Double.isNaN(time - Math.abs(angularDisplacement) / angularSpeed)) {
throw new RuntimeException(
"Next input is NaN. angularSpeed is "
+ angularSpeed
+ " angularDisplacement is "
+ angularDisplacement
+ "time is "
+ time);
}
moveNoCollision(time - Math.abs(angularDisplacement) / angularSpeed);
}
}
}