Drought.Entity.MovableEntity.move C# (CSharp) Method

        private void move()
        {
            if (!path.isFinished() && waiting == 0)
            {
                hasMoved = path.addDistance(speed);
                prevPosition.X = position.X;
                prevPosition.Y = position.Y;
                prevPosition.Z = position.Z;
                position = path.getPosition();
                prevNormal.X = normal.X;
                prevNormal.Y = normal.Y;
                prevNormal.Z = normal.Z;
                normal = levelInfo.getNormal(position.X, position.Y);
                normal.Normalize();

                heading = position - prevPosition;
                heading.Normalize();

                setOrientation();
            }
            if (waiting > 0) waiting--;
        }