Game Development Community

Math!

by Robert Fritzen · in Torque 3D Beginner · 01/31/2014 (10:48 am) · 9 replies

Ok, so who wants to be the genius today and help me with a little annoying trigonometry problem.

I'm trying to determine the radian angle between a map's "true north" vector <0 1 0> and the player's forward vector.

My current implementation looks like this:
F32 getRotationAngle(VectorF v1, VectorF v2) {
   if(!v1.isUnitLength()) {
      v1.normalize();
   }
   if(!v2.isUnitLength()) {
      v2.normalize();
   }
   //----------------------------
   F32 dot = v1.x * v2.x + v1.y * v2.y + v1.z + v2.z;
   return mAcos(dot);
}

I'm passing the value to the function like so:
VectorF tNorth, fVec;
   tNorth.set(0, 1, 0);
   tNorth.normalize();
   fVec.set(player->getTransform().getForwardVector());
   fVec.normalize();
   F32 angle = getRotationAngle(tNorth, fVec);

What I currently have is a problem where as I'm rotating it appears to hang up after it reaches a certain value and then goes backwards (I believe 180). I've also tried the arctangent method without any luck. Does anyone know what I'm doing wrong?

EDIT: For application purposes, we can safely assume they are 2D vectors. Here's a diagram:
staff.phantomdev.net/phantom139/images/Diagram.png

#1
01/31/2014 (11:10 am)
I ran into this problem when I was doing a radar that rotates with the character. I forget the specifics but basically when you go past a certain angle you need to account for the fact that the angle you're getting back is no longer coming from 0 radian. Here's the code where I account for that:

F32 targetAngle = mAtan(y /x);
if ( x < 0 ) 
    targetAngle += 3.14159265f;
#2
01/31/2014 (12:24 pm)
My problem seems to arise out of the fact when one of the player vector's "sign" switches direction:

Angle Debug:: PlayerVec <0.028856 -0.999584>, SpinAngle: -1.556366
Angle Debug:: PlayerVec <0.028856 -0.999584>, SpinAngle: -1.556366
Angle Debug:: PlayerVec <-0.001628 -0.999999>, SpinAngle: 1.569982
Angle Debug:: PlayerVec <-0.019939 -0.999801>, SpinAngle: 1.560826

Notice how the angle does a jump in the sign as well?

I also updated my code to follow what you had above and it's still doing this, any pointers?
F32 getRotationAngle(VectorF v1, VectorF v2) {
	if(!v1.isUnitLength()) {
		v1.normalize();
	}
	if(!v2.isUnitLength()) {
		v2.normalize();
	}
	//----------------------------
	F32 dX = v2.x - v1.x;
	F32 dY = v2.y - v1.y;
	F32 tAng = mAtan2(dY, dX);
	if(dX < 0) {
		tAng += Float_Pi;
	}
	return tAng;
}
#3
01/31/2014 (12:55 pm)
I suck at explaining things, so bare with me haha.

When you use those inverse functions and you get past 180 degrees, you start getting results that are coming clockwise on the unit circle. So you get things like:

X: 1, Y: 0.5, theta = 30 something degrees.
X: -1, Y: 0.5, theta = 150 something degrees.

and then we pass 180 and get:

X: -1, Y: -0.5, theta = -150 something degrees. You'd expect ~210 deg
X: 1, Y: -0.5, theta = -30 something degrees. You'd expect ~330 deg

In this example I gave above I would correct with:

if y < 0:
angle = (360 deg) + theta

And then I'd get a full 360 reading back out. I hope this makes sense haha.
#4
01/31/2014 (1:00 pm)
Got it. I went back into my old Physics book and found out that I could instead use a Dot product / Determinant relationship to solve this problem.

Here's my final code for anyone who might need something like this in the future.
F32 getRotationAngle(VectorF v1, VectorF v2) {
	//v1 and v2 must be normalizaed
	if(!v1.isUnitLength()) {
		v1.normalize();
	}
	if(!v2.isUnitLength()) {
		v2.normalize();
	}
	//----------------------------
	F32 Dot = v1.x * v2.x + v1.y * v2.y;
	F32 Det = v1.x * v2.y - v1.y * v2.x;
	F32 tAng = mAtan2(Dot, Det);

	return tAng;
}

EDIT: Apparently we posted at around the same time. This still does a sign flip at 180 (note here):
Angle Debug:: V1 <0.000000 1.000000> V2 <0.999440 0.033455>, Angle: 3.108131 (178.082780)
Angle Debug:: V1 <0.000000 1.000000> V2 <0.999996 0.002974>, Angle: 3.138619 (179.829614)
Angle Debug:: V1 <0.000000 1.000000> V2 <0.999772 -0.021377>, Angle: -3.120214 (-178.775115)
Angle Debug:: V1 <0.000000 1.000000> V2 <0.999772 -0.021377>, Angle: -3.120214 (-178.775115)

Which of course you could simply fix by adding 360 at that point, but for my application purposes, it runs fine now, thanks for the help.

EDIT #2: Anyone who wants the >180 fix, just stick this in before the return:
if(tAng < 0.f)
		tAng += Float_Pi;
#5
01/31/2014 (2:55 pm)
Was going to mention the old Dot/Det answer and yeah, there's the flip at 180 - and T2D has a VectorDot() (or some similar) function that might be handy as an addition to the engine....
#6
01/31/2014 (3:07 pm)
I'm not certain if you desired the result in the range 0 to 2Pi or -Pi to Pi, but if you're always measuring against true north, the function could be reduced to:
F32 getRotationFromTrueNorth(VectorF v1) {
   // v1 must be normalized
   if(!v1.isUnitLength()) {  
      v1.normalize();  
   }  
   //return ( v1.x < 0.0f ) ? -mAcos(v1.y) : mAcos(v1.y); // Results range -pi to pi, CW from true south
   return ( v1.x < 0.0f ) ? M_2PI_F-mAcos(v1.y) : mAcos(v1.y); // Results range 0 to 2pi, CW from true north
}
Something to consider if the function will be called often.
#7
01/31/2014 (3:33 pm)
Edit to the above, that will only work when v1.z == 0.0. Which will be almost always for a player, but to be safe it needs to set v1.z = 0.0 before the unit length test and use normalizeSafe() just in case.
F32 Player::getRotationFromTrueNorth(VectorF v1) {
   // v1 must be normalized
   v1.z = 0.0f;
   if(!v1.isUnitLength()) {  
      v1.normalizeSafe();  
   }  
   //return ( v1.x < 0.0f ) ? -mAcos(v1.y) : mAcos(v1.y); // Results range -pi to pi, CW from true south
   return ( v1.x < 0.0f ) ? M_2PI_F-mAcos(v1.y) : mAcos(v1.y); // Results range 0 to 2pi, CW from true north
}
#8
02/01/2014 (12:14 am)
This is what I use when I want compute angles between any two vectors.

DefineConsoleFunction( VectorsAng, F32, (VectorF u, VectorF v ),,
	"Compute the angle between two vectors.n")
{
	F32 NormU = u.len();	// |u|
	F32 NormV = v.len();	// |v|
	F32 Product = NormU*NormV;	// |u||v|
	if(Product==0.0f)	return 0.0f;
	F32 OneOverProduct = 1.0f / Product;

	// Cosinus
	F32 Cosinus = mDot(u,v) * OneOverProduct;

	// Sinus
	Point3F w = mCross(u,v);
	F32 NormW = w.len();

	F32 AbsSinus = NormW * OneOverProduct;

	// Remove degeneracy
	if(AbsSinus > 1.0f) AbsSinus = 1.0f;
	if(AbsSinus < -1.0f) AbsSinus = -1.0f;

	if(Cosinus>=0.0f)	return asinf(AbsSinus);
	else				return (M_PI_F-asinf(AbsSinus));
}
#9
02/01/2014 (2:03 pm)
This is what im using for 360 character movement.

float RPGPlayer::CalcTheta( const Point3F Point1, const Point3F Point2 )
{
	float Theta;
	if ( Point2.x - Point1.x == 0 ){
		if ( Point2.y > Point1.y ){
			Theta = 0;
		}else{
			Theta = static_cast<float>( M_PI_F );
		}
	}else{
		Theta = std::atan( (Point2.y - Point1.y) / (Point2.x - Point1.x) );
		if ( Point2.x > Point1.x ){
			Theta = static_cast<float>( M_PI_F ) / 2.0f - Theta;
		}else{
			Theta = static_cast<float>( M_PI_F ) * 1.5f - Theta;
		}
	}
	return Theta;
}




	//CHANGE FOR NODE ROTATION
	if(move->y||move->x){
		Point3F Point1;
		Point3F Point2;
		
		Point1.x=10;
		Point1.y=10;
		Point1.z=0;

		Point2.x=10+(move->y);
		Point2.y=10+move->x;
		Point2.z=0;
		
		//(CalcTheta(Point1,Point2)*180)/M_PI_F; // angle in degrees
		mDataBlock->DIR_ANGLE = CalcTheta(Point1,Point2);
		Point3F _pos;
		_pos.set(0,0,0);

		Point3F _axis;
		_axis.set(0,0,1);
	
		F32 y2 = move->yaw/2;
		if (y2 > M_PI_F)
		y2 -= M_2PI_F;
		mDataBlock->DIR_ANGLE-=y2;
		if (mDataBlock->DIR_ANGLE <0)
		mDataBlock->DIR_ANGLE += M_2PI_F;
		
		QuatF swimROT(EulerF(0.0f , 90 , mDataBlock->DIR_ANGLE));    
		swimROT.normalize(); 

		String _name;
		_name="BipCOM";
		mDataBlock->mShape->setNodeTransform(_name, _pos,swimROT);
		//Con::printf("Node _pos: %f",((CalcTheta(Point1,Point2)*180)/M_PI_F));
		//Con::printf("Node _pos: %f %f %f ",move->x,move->y,move->z);
		//Con::printf("Node _pos: %f %f %f : < %f    :::  %f",_rot.x,_rot.y,_rot.z,_rot.w, mDataBlock->DIR_ANGLE);
	}else{
		Point3F _pos;
		_pos.set(0,0,0);

		Point3F _axis;
		_axis.set(0,0,1);
	
		F32 y2 = move->yaw/2;
		if (y2 > M_PI_F)
		y2 -= M_2PI_F;
		mDataBlock->DIR_ANGLE-=y2;
		if (mDataBlock->DIR_ANGLE <0)
		mDataBlock->DIR_ANGLE += M_2PI_F;
		
		QuatF swimROT(EulerF(0.0f , 0.0f , mDataBlock->DIR_ANGLE));    

		//needed if idle swim has an angel.
		//QuatF swimROT(EulerF(0.0f , 90 , mDataBlock->DIR_ANGLE));    
		swimROT.normalize(); 

		String _name;
		_name="BipCOM";
		mDataBlock->mShape->setNodeTransform(_name, _pos,swimROT);

	}