I've been banging my head on this for days, searched the forums high and low, bought a math book and I'm still stuck. Ok. So I've admitted my stupidity. I feel better. :lol:
Here's what I'm trying to do. I'm trying to build a method that will be used for autopilot navigation to a target and for missile guidance to a target in 3d space. It needs to accept a target translation vector and then fire the appropriate thrusters for the physics engine. So I need it to tell me what angular velocities to apply, ie pitch up, yaw right, to intercept the target.
I know that you can determine the direction to the target by taking the world translation of the target and subtracting by the world translation of the ship or missile. Ok. Fine, that gives me a target direction vector.
Then what? I know I need to compare the rotation of the chasing object to the target direction and that should provide me with error signals. But how? What I think might work is:
Matrix3f errorMatrix = targetRotDir.invert().mult(ship.getWorldRotation().toRotationMatrix());
Then roll is errorMatrix(3,2), pitch is (1,3), and yaw is (2,1). Does that sound right? If so, how do I get the targetRotDir Matrix3f from my direction vector?
Any help from the math wiz community would be much appreciated. I still stumble from time to time on the math side of things.
Well I think I got it. Might need some further tests, but tracking several targets from different aspects seemed to work. The idea is to correct roll then pitch and yaw. Seems to work every time. If any math guru sees a problem with this method, please let me know.
Vector3f targetV = new Vector3f();
targetV.set(target.getWorldTranslation()).subtractLocal(ship.getWorldTranslation());
targetV.normalizeLocal();
float el = FastMath.asin(targetV.y);
float az = FastMath.atan2(targetV.x, targetV.z);
float elCos = FastMath.cos(el);
float azCos = FastMath.cos(az);
float elSin = FastMath.sin(el);
float azSin = FastMath.sin(az);
Matrix3f desired = new Matrix3f();
desired.m00 = azCos;
desired.m01 = azSin * -elSin;
desired.m02 = azSin * elCos;
desired.m10 = 0;
desired.m11 = elCos;
desired.m12 = elSin;
desired.m20 = -azSin;
desired.m21 = azCos * -elSin;
desired.m22 = azCos * elCos;
Matrix3f errorMat = desired.invert().mult(ship.getWorldRotation().toRotationMatrix());
String pitchCommand;
String yawCommand;
String rollCommand;
if (errorMat.m21 < 0.1f && errorMat.m21 > -0.1f && errorMat.m22 > 0){
pitchCommand = "on target";
} else if (errorMat.m21 > 0){
pitchCommand = "up";
} else {
pitchCommand = "down";
}
if (errorMat.m02 < 0.1f && errorMat.m02 > -0.1f && errorMat.m00 > 0){
yawCommand = "on target";
} else if (errorMat.m02 > 0){
yawCommand = "right";
} else {
yawCommand = "left";
}
if (errorMat.m10 < 0.1f && errorMat.m10 > -0.1f && errorMat.m11 > 0){
rollCommand = "on target";
} else if (errorMat.m10 > 0){
rollCommand = "roll left";
} else {
rollCommand = "roll right";
}