I’m running into some fiddly rounding errors in Quaternion.fromRotationMatrix and my vector math is not at the top of its game, so perhaps some other math whiz around here can shed light on the problem.
First the goodies: some code that computes a ballistic path for a projectile and orients it in the direction of the path:
package jmetest.effects;
import com.jme.app.SimpleGame;
import com.jme.renderer.ColorRGBA;
import com.jme.scene.Controller;
import com.jme.scene.Node;
import com.jme.scene.shape.Box;
import com.jme.scene.shape.Disk;
import com.jme.scene.shape.Quad;
import com.jme.scene.state.LightState;
import com.jme.bounding.BoundingBox;
import com.jme.image.Texture;
import com.jme.math.FastMath;
import com.jme.math.Matrix3f;
import com.jme.math.Quaternion;
import com.jme.math.Vector3f;
public class TestBallistic extends SimpleGame
{
public static void main (String[] args)
{
TestBallistic app = new TestBallistic();
app.setDialogBehaviour(ALWAYS_SHOW_PROPS_DIALOG);
app.start();
}
protected void simpleUpdate ()
{
}
protected void simpleInitGame ()
{
display.setTitle("Ballistic Path Test");
float dist = 10;
// set up the camera
Vector3f loc = new Vector3f(0, -dist, 10);
cam.setLocation(loc);
Matrix3f rotm = new Matrix3f();
rotm.fromAngleAxis(-FastMath.PI/4, cam.getLeft());
rotm.multLocal(cam.getDirection());
rotm.multLocal(cam.getUp());
rotm.multLocal(cam.getLeft());
cam.update();
for (int yy = -1; yy < 1; yy++) {
for (int xx = -1; xx < 1; xx++) {
Quad quad = new Quad("ground", 10, 10);
quad.setLightCombineMode(LightState.OFF);
int index = (yy+1)*2 + (xx+1);
quad.setSolidColor(COLORS[index]);
quad.setLocalTranslation(
new Vector3f(xx * 10 + 5, yy * 10 + 5, -1));
rootNode.attachChild(quad);
}
}
for (int yy = -1; yy <= 1; yy++) {
for (int xx = -1; xx <= 1; xx++) {
setup(xx * dist, yy * dist);
}
}
}
protected void setup (float x, float y)
{
// create the target
Box target = new Box("target", new Vector3f(-.1f, -.1f, -.1f),
new Vector3f(.1f, .1f, .1f));
target.setLocalTranslation(new Vector3f(x, y, 0));
rootNode.attachChild(target);
// and the projectile
Box box = new Box("box", new Vector3f(-1, -0.5f, -0.25f),
new Vector3f(1, 0.5f, 0.25f));
Disk disk = new Disk("dot", 10, 10, 0.5f);
disk.setLightCombineMode(LightState.OFF);
disk.setLocalTranslation(new Vector3f(0.5f, 0f, 0.3f));
Node shot = new Node("shot");
shot.attachChild(box);
shot.attachChild(disk);
rootNode.attachChild(shot);
// start with the "launcher" pointed along the x axis
Vector3f diff = target.getLocalTranslation().subtract(
shot.getLocalTranslation());
Vector3f vel = diff.normalize();
float range = diff.length(), angle = -FastMath.PI/4;
Vector3f axis = UP.cross(vel);
axis.normalizeLocal();
// rotate it up (around the y axis) the necessary elevation
Quaternion rot = new Quaternion();
rot.fromAngleAxis(angle, axis);
rot.multLocal(vel);
// give the cannon its muzzle velocity
float muzvel = FastMath.sqrt(range * G / FastMath.sin(2*angle));
vel.multLocal(muzvel);
Vector3f start = new Vector3f(0, 0, 0);
float time = computeFlightTime(range, muzvel, angle);
OrientingBallisticPath path =
new OrientingBallisticPath(shot, start, vel, GRAVITY, time);
shot.addController(path);
path.setActive(true);
}
/**
* Computes and returns the flight time of a projectile launched at a
* particular angle.
*/
public static float computeFlightTime (float range, float vel, float angle)
{
return range / (vel * FastMath.cos(angle));
}
protected static class BallisticPath extends Controller
{
public BallisticPath (Node node, Vector3f start, Vector3f velocity,
Vector3f accel, float duration)
{
_node = node;
_curpos = start;
_velocity = velocity;
_accel = accel;
_duration = duration;
}
public void update (float time)
{
// adjust the position
_velocity.mult(time, _temp);
_curpos.addLocal(_temp);
_node.setLocalTranslation(_curpos);
// check to see if we're done
_accum += time;
if (_accum >= _duration) {
setActive(false);
} else {
// adjust our velocity due to acceleration
_accel.mult(time, _temp);
_velocity.addLocal(_temp);
}
}
protected Node _node;
protected Vector3f _curpos, _velocity, _accel;
protected float _duration, _accum;
protected Vector3f _temp = new Vector3f(0, 0, 0);
}
protected static class OrientingBallisticPath extends BallisticPath
{
public OrientingBallisticPath (
Node node, Vector3f start, Vector3f velocity, Vector3f accel,
float duration)
{
super(node, start, velocity, accel, duration);
// compute the up vector (opposite of acceleration)
_up = accel.negate();
_up.normalizeLocal();
}
public void update (float time)
{
super.update(time);
// the velocity coincides with the x axis
_axes[0].set(_velocity);
_axes[0].normalizeLocal();
// cros the velocity with the up vector to get a vector
// orthogonal to both
_axes[0].cross(_up, _axes[1]);
_axes[1].normalizeLocal();
// cross said orthogonal vector with the x to get the new z
_axes[1].cross(_axes[0], _axes[2]);
// then cross z with x to get the new y
_axes[2].cross(_axes[0], _axes[1]);
// compute a rotation to this new coordinate system
_rotate.fromAxes(_axes);
_node.setLocalRotation(_rotate);
}
protected Vector3f _up;
protected Quaternion _rotate = new Quaternion();
}
protected static Vector3f[] _axes = {
new Vector3f(), new Vector3f(), new Vector3f() };
protected static final float G = -9.8f;
protected static final Vector3f GRAVITY = new Vector3f(0, 0, G);
protected static final Vector3f UP = new Vector3f(0, 0, 1);
protected static final ColorRGBA[] COLORS = {
ColorRGBA.red, ColorRGBA.green, ColorRGBA.blue, ColorRGBA.gray };
}
Note that the code assumes that the projectile is "oriented" in the positive x direction as it then computes a rotation matrix that rotates the x axis to coincide with the direction of the velocity vector. I originally tried computing an arbitrary rotation using Quaternion.fromAngleAxis() between some arbitrary orientation of the projectile and the velocity vector, but I got weird rotation of the projectile, but that's not related to this problem.
If you run the above code, you'll see that one of the eight orientations displays a bunch of weird jittering. I traced that down to the following code in Quaternion.java (which is called from Quaternion.fromAxes() which I call to compute the orienting rotation):
public void fromRotationMatrix(Matrix3f matrix) {
float t = matrix.m00 + matrix.m11 + matrix.m22 + 1;
if (t > 0f) {
float s = 0.5f / FastMath.sqrt(t);
w = 0.25f / s;
x = (matrix.m21 - matrix.m12) * s;
y = (matrix.m02 - matrix.m20) * s;
z = (matrix.m10 - matrix.m01) * s;
} else if ((matrix.m00 > matrix.m11) && (matrix.m00 > matrix.m22)) {
float s = FastMath
.sqrt(1.0f + matrix.m00 - matrix.m11 - matrix.m22) * 2;
x = 0.25f * s;
y = (matrix.m01 + matrix.m10) / s;
z = (matrix.m02 + matrix.m20) / s;
w = (matrix.m12 - matrix.m21) / s;
} else if (matrix.m11 > matrix.m22) {
float s = FastMath
.sqrt(1.0f + matrix.m11 - matrix.m00 - matrix.m22) * 2;
x = (matrix.m01 + matrix.m10) / s;
y = 0.25f * s;
z = (matrix.m12 + matrix.m21) / s;
w = (matrix.m02 - matrix.m20) / s;
} else {
float s = FastMath
.sqrt(1.0f + matrix.m22 - matrix.m00 - matrix.m11) * 2;
x = (matrix.m02 + matrix.m20) / s;
y = (matrix.m12 + matrix.m21) / s;
z = 0.25f * s;
w = (matrix.m01 - matrix.m10) / s;
}
}
It turns out that the jittering path is such that t in the above computation is really close to zero. It should be zero, but due to floating point rounding errors, it is some times just a smidgen above zero which causes the ensuing calculations to break and the rotation Quaternion computed is all zeros. Not knowing enough about calculating Quaternions from rotation matrices, I don't know why there are four code paths. If it's just for optimization, then perhaps it is OK to change that to some epsilon greater than zero (which I tried and everything works lovely, in this particular case), but perhaps my problem should be fixed somewhere else.
Anyone have suggestions?