Universe.Physics.BulletSPlugin.BSActorMoveToTarget.Mover2 C# (CSharp) Method

        void Mover2(float timeStep)
        {
            // Don't do hovering while the object is selected.
            if (!isActive)
                return;

            OMV.Vector3 origPosition = m_controllingPrim.RawPosition;     // DEBUG DEBUG (for printout below)
            OMV.Vector3 addedForce = OMV.Vector3.Zero;

            // CorrectionVector is the movement vector required this step
            OMV.Vector3 correctionVector = m_targetMotor.Step(timeStep, m_controllingPrim.RawPosition);

            // If we are very close to our target, turn off the movement motor.
            if (m_targetMotor.ErrorIsZero())
            {
                m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover3,zeroMovement,pos={1},mass={2}",
                                        m_controllingPrim.LocalID, m_controllingPrim.RawPosition, m_controllingPrim.Mass);
                m_controllingPrim.ForcePosition = m_targetMotor.TargetValue;
                m_controllingPrim.ForceVelocity = OMV.Vector3.Zero;
                // Setting the position does not cause the physics engine to generate a property update. Force it.
                m_physicsScene.PE.PushUpdate(m_controllingPrim.PhysBody);
            }
            else
            {
                // First force to move us there -- the motor return a timestep scaled value.
                addedForce = correctionVector / timeStep;
                // Remove the existing velocity (only the moveToTarget force counts)
                addedForce -= m_controllingPrim.RawVelocity;
                // Overcome gravity. 
                addedForce -= m_controllingPrim.Gravity;

                // Add enough force to overcome the mass of the object
                addedForce *= m_controllingPrim.Mass;

                m_controllingPrim.AddAngularForce(addedForce, false /* pushForce */, true /* inTaintTime */);
            }
            m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover3,move,fromPos={1},addedForce={2}",
                                            m_controllingPrim.LocalID, origPosition, addedForce);
        }
    }