Turn Toward Orientation

[JohnHardy]

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: Select all

            // 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]

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

[anthrax11]

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: Select all

    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]

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]

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