/**
* <code>rotateUpTo</code> is a utility function that alters the local rotation to point the Y
* axis in the direction given by newUp.
*
* @param newUp the up vector to use - assumed to be a unit vector.
*/
public void rotateUpTo(Vector3f newUp) {
TempVars vars = TempVars.get();
Vector3f compVecA = vars.vect1;
Quaternion q = vars.quat1;
// First figure out the current up vector.
Vector3f upY = compVecA.set(Vector3f.UNIT_Y);
Quaternion rot = localTransform.getRotation();
rot.multLocal(upY);
// get angle between vectors
float angle = upY.angleBetween(newUp);
// figure out rotation axis by taking cross product
Vector3f rotAxis = upY.crossLocal(newUp).normalizeLocal();
// Build a rotation quat and apply current local rotation.
q.fromAngleNormalAxis(angle, rotAxis);
q.mult(rot, rot);
vars.release();
setTransformRefresh();
}
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;
}