/**
* Copyright (C) BFH www.bfh.ch 2011
* Code written by: Patrick Dobler, Marc Folly
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
package ch.bfh.ti.kybernetik.engine.controller.roboter;
import javax.vecmath.Point2d;
import javax.vecmath.Vector2d;
import ch.bfh.ti.kybernetik.engine.VecMathUtils;
import ch.bfh.ti.kybernetik.engine.controller.Simulator;
import ch.bfh.ti.kybernetik.engine.model.Roboter;
import ch.bfh.ti.kybernetik.engine.model.RoboterElement;
/**
* The {@link Simulator} default {@link RoboterConstruction} implementation
*
*/
public class DefaultRoboterConstructionImpl implements RoboterConstruction {
private final Roboter roboter;
public DefaultRoboterConstructionImpl(Roboter roboter) {
super();
this.roboter = roboter;
}
@Override
public Point2d getRoboterElementPoint(RoboterElement roboterElements) {
return getRoboterRoboterElementPoint(new Point2d(roboter.getX(), roboter.getY()), roboter.getDirection(),
roboterElements.getDistanceX());
}
private Point2d getRoboterRoboterElementPoint(Point2d roboterPoint, Vector2d direction, double distance) {
Vector2d horiziontalVector = buildRightAngleVector(direction);
double x = roboterPoint.getX() + (distance) * horiziontalVector.getX();
double y = roboterPoint.getY() + (distance) * horiziontalVector.getY();
return new Point2d(x, y);
}
@Override
public Vector2d getRoboterElementAngleVector(RoboterElement roboterElement) {
double sensorAngle = roboterElement.getElementAngle();
Vector2d sensorVector = VecMathUtils.rotateVector(roboter.getDirection(), sensorAngle);
return sensorVector;
}
private Vector2d buildRightAngleVector(Vector2d vector) {
Vector2d horizontalVector = new Vector2d(-vector.getY(), vector.getX());
horizontalVector.normalize();
return horizontalVector;
}
}