Revision e8e79779

View differences:

utility/matrix.h
216 216
        return ret;
217 217
    }
218 218

  
219
    double trace() const
220
    {
221
        double tr = 0.0;
222
        for (int i = 0; i < N; ++i)
223
            tr += cell(i, i);
224
        return tr;
225
    }
226

  
219 227
private:
220 228
    double _cell_data[N*N];
221 229
};
utility/quaternion.h
126 126
        _z = axis.z() * sht;
127 127
    }
128 128

  
129
    void fromMatrix(Matrix<3> m)
129
    void fromMatrix(const Matrix<3>& m)
130 130
    {
131
        float tr = m(0, 0) + m(1, 1) + m(2, 2);
131
        double tr = m.trace();
132 132

  
133
        float S = 0.0;
133
        double S;
134 134
        if (tr > 0)
135 135
        {
136 136
            S = sqrt(tr+1.0) * 2;
......
139 139
            _y = (m(0, 2) - m(2, 0)) / S;
140 140
            _z = (m(1, 0) - m(0, 1)) / S;
141 141
        }
142
        else if ((m(0, 0) < m(1, 1))&(m(0, 0) < m(2, 2)))
142
        else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2))
143 143
        {
144 144
            S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2;
145 145
            _w = (m(2, 1) - m(1, 2)) / S;
......
147 147
            _y = (m(0, 1) + m(1, 0)) / S;
148 148
            _z = (m(0, 2) + m(2, 0)) / S;
149 149
        }
150
        else if (m(1, 1) < m(2, 2))
150
        else if (m(1, 1) > m(2, 2))
151 151
        {
152 152
            S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;
153 153
            _w = (m(0, 2) - m(2, 0)) / S;

Also available in: Unified diff