## Get Current Angle Of Revolute Joint

Discuss any questions about BEPUphysics or problems encountered.
squimball
Posts: 7
Joined: Sat May 07, 2011 9:25 pm

### Get Current Angle Of Revolute Joint

Greetings! I have been trying my best to not post this question here, but I have spent the better part of a week trying to figure this out. I've looked through most of this forum and some come close to answering it, but not really. Also, please note that I am sort of a beginner at 3D physics stuff, so if possible, a tiny little snippet of sample code would be greatly appreciated!

I notice that in the joint limit, you can specify minimum and maximum angle. I just cannot for the life of me figure out how to retrieve the current angle of the joint. I have tried to calculate this by using the joint error and servo goal. This works somewhat, the the value gets kind of wacky when you initially move the joint. It smooths out after a frame or two.

FYI, this is an example of how I am currently attempting to do this. I have tried many variations on the following...

Code: Select all

float errorMult = 1.0f;
float errorMin = -4.0f;
float errorMax = 4.0f;

myAngle = MathHelper.WrapAngle(revJoint.Motor.Settings.Servo.Goal - MathHelper.Clamp(revJoint.Motor.Error * errorMult, errorMin, errorMax))
Yes, I'm sure you will agree that my method is quite "hacky". haha I just know there is a better way and that someone can probably answer this in like 5 seconds. LOL

Many thanks and I really love this engine!!

Norbo
Posts: 4816
Joined: Tue Jul 04, 2006 4:45 am

### Re: Get Current Angle Of Revolute Joint

RevoluteLimit and RevoluteAngularJoint do not have a single simple 'angle' in their computation, though the 'Error' property on the RevoluteMotor gets pretty close. That measures the distance between the current angle and the target goal angle if the motor is in servo mode (equivalent to the absolute value of Goal - CurrentAngle, ignoring wrapping).

There's a direct approach that works between any two directions. For two normalized vectors A and B, the angle = Acos(A dot B). In other words:

Code: Select all

double angle = Math.Acos(Vector3.Dot(A, B))
You can set up A and B yourself using the orientation of the involved entities, or grab data from the constraint bases/test axes.

squimball
Posts: 7
Joined: Sat May 07, 2011 9:25 pm

### Re: Get Current Angle Of Revolute Joint

You are most awesome! Works perfect for my situation.

Here's how I did it btw..

Code: Select all

Vector3 A = entity1.MotionState.OrientationMatrix.Forward;
Vector3 B = entity2.MotionState.OrientationMatrix.Forward;
double angle = Math.Acos(Vector3.Dot(A, B));
Thanks!!!!!!!!