Turn Toward Orientation

JohnHardy · **Joined:** Mon Mar 05, 2012 11:39 pm **Posts:** 9

Hi all,

I'm trying to write some code to make a body turn to face an orientation (perhaps derived from a point in world space) using forces and torque.

Can you give me some advice on how to do this?

Code:

            // Angular.
            Vector3 vDeltaW = Vector3.Subtract(this.vAngularVelTarget, Vector3.TransformNormal(pBody.AngularVelocity, mBodyInverse));
            if (Math.Abs(vDeltaW.X) + Math.Abs(vDeltaW.Y) + Math.Abs(vDeltaW.Z) > 0.001f)
            {
                Vector3 vLocalForceW = (vDeltaW / fTimeStep) * fMass;
                //Vector3 vLocalForceW = new Vector3(
                //    (vDeltaW.X / fTimeStep) * (1.0f / pBody.InvInertiaDiagLocal.X),
                //    (vDeltaW.Y / fTimeStep) * (1.0f / pBody.InvInertiaDiagLocal.Y),
                //    (vDeltaW.Z / fTimeStep) * (1.0f / pBody.InvInertiaDiagLocal.Z));


                Vector3 vMaxForceW = vAngularAcceleration * fMass;
                //Vector3 vMaxForceW = new Vector3(
                //   vAngularAcceleration.X * (1.0f / pBody.InvInertiaDiagLocal.X),
                //   vAngularAcceleration.Y * (1.0f / pBody.InvInertiaDiagLocal.Y),
                //   vAngularAcceleration.Z * (1.0f / pBody.InvInertiaDiagLocal.Z));

                if (vLocalForceW.X > vMaxForceW.X) vLocalForceW.X = vMaxForceW.X;
                else if (vLocalForceW.X < -vMaxForceW.X) vLocalForceW.X = -vMaxForceW.X;
                if (vLocalForceW.Y > vMaxForceW.Y) vLocalForceW.Y = vMaxForceW.Y;
                else if (vLocalForceW.Y < -vMaxForceW.Y) vLocalForceW.Y = -vMaxForceW.Y;
                if (vLocalForceW.Z > vMaxForceW.Z) vLocalForceW.Z = vMaxForceW.Z;
                else if (vLocalForceW.Z < -vMaxForceW.Z) vLocalForceW.Z = -vMaxForceW.Z;

                Vector3 vWorldForceW = Vector3.TransformNormal(vLocalForceW, pBody.WorldTransform);
                pBody.ApplyTorque(vWorldForceW);
            }

The above code makes a body rotate given a target angular velocity (with a cap based on acceleration). I'm struggling to then compute the target velocity given (a) a point in worldspace that I want to rotate to, or (b) a quaternion rotation in world space.

Thanks in advance, sorry if its an obvious question!

John

JohnHardy · **Joined:** Mon Mar 05, 2012 11:39 pm **Posts:** 9

No ideas? Or is this just really obvious and I'm not seeing it?

anthrax11 · **Joined:** Wed Feb 24, 2010 9:49 pm **Posts:** 26

I'm not sure I fully understand, but here's how I thought of it. The code extracts the body's orientation around each axis (yaw, pitch, roll) and calculates the delta towards the target orientation. The delta is applied as torque (and you probably want to fiddle with this to get the speed right). Once the orientation is right, it applies angular damping to stop the rotation.

Code:

    Vector3 GetYawPitchRoll(Matrix m)
    {
        return new Vector3(
            (float)Math.Atan(m.M21 / m.M11),
            (float)Math.Atan(-m.M31 / Math.Sqrt(m.M32 * m.M32 + m.M33 * m.M33)),
            (float)Math.Atan(m.M32 / m.M33)
        );
    }

    Matrix targetRotMatrix = Matrix.RotationQuaternion(targetRotQuat)

    Vector3 yprBefore = GetYawPitchRoll(pBody.WorldTransform);
    Vector3 yprAfter = GetYawPitchRoll(targetRotationMatrix);
    Vector3 yprDelta = yprAfter - yprBefore;

    if (yprDelta.LengthSquared() < 0.05f)
        pBody.SetDamping(pBody.LinearDamping, 0.9f);
    else
        pBody.ApplyTorque(yprDelta);

I haven't tested this much and it has some issues like it'll fail if it needs to turn 180 degrees (gimbal lock condition), but hopefully it gives you some idea that you can use.

Granyte · **Joined:** Tue Dec 27, 2011 11:51 am **Posts:** 39

I'm trying to build a look at behavior to turn a ship toward a target and then trust toward it but they somehow always faill and thrust the ship in a randoom place and they disapear.

JohnHardy · **Joined:** Mon Mar 05, 2012 11:39 pm **Posts:** 9

Seems we are in the same boat Granyte. I won't have chance to work on this for a another week, but I found some threads that look promising in the meantime:

http://answers.unity3d.com/questions/48 ... bject.html
http://www.ogre3d.org/addonforums/viewt ... f=4&t=5706 The solution at the bottom of this one is similar to anthrax11's idea

http://www.wiremod.com/forum/expression ... orial.html
http://www.wiremod.com/forum/finished-c ... light.html

Physics Simulation Forum

Turn Toward Orientation

Who is online