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:
I'm passing the value to the function like so:
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:

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:

About the author
Illinois Grad. Retired T3D Developer / Pack Dev.
#2
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?
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
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.
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
Here's my final code for anyone who might need something like this in the future.
EDIT: Apparently we posted at around the same time. This still does a sign flip at 180 (note here):
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:
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);
}
Andrew Mac
F32 targetAngle = mAtan(y /x); if ( x < 0 ) targetAngle += 3.14159265f;