public float getHeadingAtWP(int index) {
float heading = 0;
Waypoint nextWayPoint = getNextWayPoint(index);
// if next way point available, compute heading towards it
if (nextWayPoint != null) {
// compute driving direction by looking at next way point from current position
Vector3f targetPosition = nextWayPoint.getPosition().clone();
targetPosition.setY(0);
Vector3f currentPosition = waypointList.get(index).getPosition().clone();
currentPosition.setY(0);
Vector3f drivingDirection = targetPosition.subtract(currentPosition).normalize();
// compute heading (orientation) from driving direction vector for
// angle between driving direction and heading "0"
float angle0 = drivingDirection.angleBetween(new Vector3f(0, 0, -1));
// angle between driving direction and heading "90"
float angle90 = drivingDirection.angleBetween(new Vector3f(1, 0, 0));
// get all candidates for heading
// find the value from {heading1,heading2} which matches with one of {heading3,heading4}
float heading1 = (2.0f * FastMath.PI + angle0) % FastMath.TWO_PI;
float heading2 = (2.0f * FastMath.PI - angle0) % FastMath.TWO_PI;
float heading3 = (2.5f * FastMath.PI + angle90) % FastMath.TWO_PI;
float heading4 = (2.5f * FastMath.PI - angle90) % FastMath.TWO_PI;
float diff_1_3 = FastMath.abs(heading1 - heading3);
float diff_1_4 = FastMath.abs(heading1 - heading4);
float diff_2_3 = FastMath.abs(heading2 - heading3);
float diff_2_4 = FastMath.abs(heading2 - heading4);
if ((diff_1_3 < diff_1_4 && diff_1_3 < diff_2_3 && diff_1_3 < diff_2_4)
|| (diff_1_4 < diff_1_3 && diff_1_4 < diff_2_3 && diff_1_4 < diff_2_4)) {
// if diff_1_3 or diff_1_4 are smallest --> the correct heading is heading1
heading = heading1;
} else {
// if diff_2_3 or diff_2_4 are smallest --> the correct heading is heading2
heading = heading2;
}
}
return heading;
}
public FollowBox(Simulator sim, final TrafficCar vehicle, FollowBoxSettings settings) {
this.sim = sim;
this.vehicle = vehicle;
this.settings = settings;
waypointList = settings.getWayPoints();
maxDistance = settings.getMaxDistance();
motionPath = new MotionPath();
motionPath.setCycle(settings.isPathCyclic());
for (Waypoint wayPoint : waypointList) motionPath.addWayPoint(wayPoint.getPosition());
motionPath.setPathSplineType(SplineType.CatmullRom); // --> default: CatmullRom
motionPath.setCurveTension(settings.getCurveTension());
if (settings.isPathVisible())
motionPath.enableDebugShape(sim.getAssetManager(), sim.getSceneNode());
motionPath.addListener(
new MotionPathListener() {
public void onWayPointReach(MotionTrack control, int wayPointIndex) {
// set speed limit for next way point
int index = wayPointIndex % waypointList.size();
float speed = waypointList.get(index).getSpeed();
setSpeed(speed);
// if last way point reached
if (motionPath.getNbWayPoints() == wayPointIndex + 1) {
// reset vehicle to first way point if not cyclic
if (!motionPath.isCycle()) setToWayPoint(0);
}
}
});
followBox = createFollowBox();
motionControl = new MotionTrack(followBox, motionPath);
// get start way point
int startWayPointIndex = settings.getStartWayPointIndex();
// set start speed
float initialSpeed = waypointList.get(startWayPointIndex).getSpeed();
setSpeed(initialSpeed);
// set start position
setToWayPoint(startWayPointIndex);
// move object along path considering rotation
motionControl.setDirectionType(MotionTrack.Direction.PathAndRotation);
// loop movement of object
motionControl.setLoopMode(LoopMode.Loop);
// rotate moving object
// motionControl.setRotation(new Quaternion().fromAngleNormalAxis(-FastMath.HALF_PI,
// Vector3f.UNIT_Y));
// set moving object to position "20 seconds"
// motionPath.interpolatePath(20, motionControl);
// start movement
// motionControl.play(); // already contained in update method
}
public float getReducedSpeed() {
// return a temporarily reduced speed for the traffic car
// in order to reach next (lower) speed limit in time
float reducedSpeedInKmh = Float.POSITIVE_INFINITY;
// if next way point with lower speed comes closer --> reduce speed
int currentIndex = motionControl.getCurrentWayPoint();
Waypoint nextWP = getNextWayPoint(currentIndex);
if (nextWP != null) {
// current way point (already passed)
Waypoint curentWP = waypointList.get(currentIndex);
// speed at current way point
float currentSpeedInKmh = curentWP.getSpeed();
float currentSpeed = currentSpeedInKmh / 3.6f;
// speed at next way point
float targetSpeedInKmh = nextWP.getSpeed();
float targetSpeed = targetSpeedInKmh / 3.6f;
// if speed at the next WP is lower than at the current WP --> brake vehicle
if (targetSpeed < currentSpeed) {
// % of traveled distance between current and next way point
float wayPercentage = motionControl.getCurrentValue();
// distance between current and next way point
Vector3f currentPos = curentWP.getPosition().clone();
currentPos.setY(0);
Vector3f nextPos = nextWP.getPosition().clone();
nextPos.setY(0);
float distance = currentPos.distance(nextPos);
// distance (in meters) between follow box and next way point
float distanceToNextWP = (1 - wayPercentage) * distance;
// speed difference in m/s between current WP's speed and next WP's speed
float speedDifference = currentSpeed - targetSpeed;
// compute the distance in front of the next WP at what the vehicle has to start
// braking with 50% brake force in order to reach the next WP's (lower) speed in time.
float deceleration50Percent = 50f * vehicle.getMaxBrakeForce() / vehicle.getMass();
// time in seconds needed for braking process
float time = speedDifference / deceleration50Percent;
// distance covered during braking process
float coveredDistance = 0.5f * -deceleration50Percent * time * time + currentSpeed * time;
// start braking in x meters
float distanceToBrakingPoint = distanceToNextWP - coveredDistance;
if (distanceToBrakingPoint < 0) {
// reduce speed linearly beginning from braking point
// % of traveled distance between braking point and next way point
float speedPercentage = -distanceToBrakingPoint / coveredDistance;
// 0% traveled: reduced speed = currentSpeed
// 50% traveled: reduced speed = (currentSpeed+targetSpeed)/2
// 100% traveled: reduced speed = targetSpeed
float reducedSpeed = currentSpeed - (speedPercentage * speedDifference);
reducedSpeedInKmh = reducedSpeed * 3.6f;
/*
if(vehicle.getName().equals("car1"))
{
float vehicleSpeedInKmh = vehicle.getLinearSpeedInKmh();
System.out.println(curentWP.getName() + " : " + speedPercentage + " : " +
reducedSpeedInKmh + " : " + vehicleSpeedInKmh + " : " + targetSpeedInKmh);
}
*/
}
}
}
return reducedSpeedInKmh;
}
