Angle between vectors in a given plane

[Zatnikitelman]

I'm a little stuck working on the autopilot for my Prometheus. I've made a little more progress in a combined vector system to work on all three axes at once. Here's the code I'm working with so far:

                        VECTOR3 attitude = _V(0, 0, 0);
			double x = 70*RAD; //pitch
			double y = 90*RAD; //heading
			double z = attitude.z;

			MATRIX3 rotxmatrix = _M(1, 0, 0,
									0, cos(x), sin(x),
									0, -sin(x), cos(x));

			MATRIX3 rotymatrix = _M(cos(y), 0, sin(y),
									0, 1, 0,
									-sin(y), 0, cos(y));

			VECTOR3 tgtDirVector = mul(rotxmatrix, _V(0, 0, 1));
			tgtDirVector = mul(rotymatrix, _V(tgtDirVector.x, tgtDirVector.y, tgtDirVector.z));

			VECTOR3 tgtRotVector = mul(rotxmatrix, _V(0, 1, 0));
			tgtRotVector = mul(rotymatrix, _V(tgtRotVector.x, tgtRotVector.y, tgtRotVector.z));

			VECTOR3 locTgtDirVector;
			VECTOR3 locTgtRotVector;
			HorizonInvRot(tgtDirVector, locTgtDirVector);
			HorizonInvRot(tgtRotVector, locTgtRotVector);

                        //This is the core of the problem I think here
			double dPitchError = 0.0;
			VECTOR3 vPitch = _V(0, 0, 1);
			dPitchError = acos((vPitch.y * locTgtDirVector.y) + (vPitch.z * locTgtDirVector.z));
                        
                        //This is the PID loop, the PID itself works fine. This particular one just takes a current error value (in degrees for now)
			double pitchval = pids[0]->updateError(dPitchError*DEG, time, true);
			this->SetAttitudeRotLevel(0, pitchval);

Right now I'm only focusing on the pitch axis, I'll add roll and yaw once I get this working. But basically I'm looking for the angle between the vector I want the launcher to point at (defined by the rotx and roty matrices) and the vector my vessel is currently at. I use both a dir and a rot vector since just a single vector would leave at least one axis (roll) undefined. The problem might be how I'm getting the angle, and thus the error, between the two vectors. Pitch takes place within the Y-Z plane so that's what I'm doing there after the dPitchError line. I read online that one way to get the angle between vectors in a plane is to take the dot product (y^2+z^2) then the arccosine of that. That somewhat seems to work, but as my vehicle pitches over, the error only grows. What am I missing here? Is there a better way entirely?

Thanks!

 

[Hlynkacg]

Assuimg a shared origin point, you will need 3 vectors. the two vectors you're measuring "A" and "B" and the "H" which is the normal vector of the plane you want to project them on.

Assuming H has a magnitude of 1. Get the dot product of A and H and then subtract H * dot product from A to get "A2", Repeat this process for B, and then measure the angle between A2 and B2.

 

[Quick_Nick]

I believe you'll want to do your heading rotation before your pitch rotation.

 

[Face]

But basically I'm looking for the angle between the vector I want the launcher to point at (defined by the rotx and roty matrices) and the vector my vessel is currently at. I use both a dir and a rot vector since just a single vector would leave at least one axis (roll) undefined. The problem might be how I'm getting the angle, and thus the error, between the two vectors. Pitch takes place within the Y-Z plane so that's what I'm doing there after the dPitchError line. I read online that one way to get the angle between vectors in a plane is to take the dot product (y^2+z^2) then the arccosine of that. That somewhat seems to work, but as my vehicle pitches over, the error only grows.

I've had a similar deviation calculation need in [ame="http://www.orbithangar.com/searchid.php?ID=2609"]JumpDriveMFD[/ame]. Perhaps you'll find something in the code there that fits your need.

 

[jedidia]

I have a function for calculating the angle between two vectors lying around in IMS2, originally written by v_champ, and also a function to rotate a vector around an arbitrary axis, originally written by Orb, if memory serves me right. hope that helps (sorry for the lack of documentation... that code is so old I haven't looked at it in ages):

double GetAngle(const VECTOR3& v1, const VECTOR3& v2, const VECTOR3& signRef)
{
	double angle = (float)acos(dotp(v1, v2) / (length(v1) * length(v2)));
	if (dotp(signRef, crossp(v1, v2)) < 0)
	{
		angle = -angle;
	}
	return angle;
}


void RotateVector(VECTOR3& v, const VECTOR3& axis, const double angle) {
	double c = cos(angle); double s = sin(angle); double C = 1 - c;
	double xs = axis.x * s; double ys = axis.y * s; double zs = axis.z * s;
	double xC = axis.x * C; double yC = axis.y * C; double zC = axis.z * C;
	double xyC = axis.x * yC; double yzC = axis.y * zC; double zxC = axis.z * xC;
	MATRIX3 m = _M(
		axis.x * xC + c, xyC - zs, zxC + ys,
		xyC + zs, axis.y * yC + c, yzC - xs,
		zxC - ys, yzC + xs, axis.z * zC + c);
	v = mul(m, v);
}